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.60 2010/08/12 19:11:22 andgi 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   if (FGModel::Run()) return true;  // Fast return if we have nothing to do ...
 263   if (FDMExec->Holding()) return false;
 264
 265   double dt = FDMExec->GetDeltaT()*rate;  // The 'stepsize'
 266
 267   RunPreFunctions();
 268
 269   // Calculate state derivatives
 270   CalculatePQRdot();           // Angular rate derivative
 271   CalculateUVWdot();           // Translational rate derivative
 272   ResolveFrictionForces(dt);   // Update rate derivatives with friction forces
 273   CalculateQuatdot();          // Angular orientation derivative
 274   CalculateInertialVelocity(); // Translational position derivative
 275
 276   // Propagate rotational / translational velocity, angular /translational position, respectively.
 277   Integrate(VState.vPQRi,             vPQRdot,           VState.dqPQRdot,           dt, integrator_rotational_rate);
 278   Integrate(VState.vUVW,              vUVWdot,           VState.dqUVWdot,           dt, integrator_translational_rate);
 279   Integrate(VState.qAttitudeECI,      vQtrndot,          VState.dqQtrndot,          dt, integrator_rotational_position);
 280   Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
 281
 282   VState.qAttitudeECI.Normalize(); // Normalize the ECI Attitude quaternion
 283
 284   VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle()); // Update the Earth position angle (EPA)
 285
 286   // Update the "Location-based" transformation matrices from the vLocation vector.
 287
 288   Ti2ec = GetTi2ec();          // ECI to ECEF transform
 289   Tec2i = Ti2ec.Transposed();  // ECEF to ECI frame transform
 290   Tl2ec = GetTl2ec();          // local to ECEF transform
 291   Tec2l = Tl2ec.Transposed();  // ECEF to local frame transform
 292   Ti2l  = GetTi2l();
 293   Tl2i  = Ti2l.Transposed();
 294
 295   // Update the "Orientation-based" transformation matrices from the orientation quaternion
 296
 297   Ti2b  = GetTi2b();           // ECI to body frame transform
 298   Tb2i  = Ti2b.Transposed();   // body to ECI frame transform
 299   Tl2b  = Ti2b*Tl2i;           // local to body frame transform
 300   Tb2l  = Tl2b.Transposed();   // body to local frame transform
 301   Tec2b = Tl2b * Tec2l;        // ECEF to body frame transform
 302   Tb2ec = Tec2b.Transposed();  // body to ECEF frame tranform
 303
 304   // Set auxililary state variables
 305   VState.vLocation = Ti2ec*VState.vInertialPosition;
 306   RecomputeLocalTerrainRadius();
 307
 308   VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
 309   radInv = 1.0/VehicleRadius;
 310
 311   VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
 312
 313   VState.qAttitudeLocal = Tl2b.GetQuaternion();
 314
 315   // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
 316   vVel = Tb2l * VState.vUVW;
 317
 318   RunPostFunctions();
 319
 320   Debug(2);
 321   return false;
 322 }
 323
 324 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 325 // Compute body frame rotational accelerations based on the current body moments
 326 //
 327 // vPQRdot is the derivative of the absolute angular velocity of the vehicle
 328 // (body rate with respect to the inertial frame), expressed in the body frame,
 329 // where the derivative is taken in the body frame.
 330 // J is the inertia matrix
 331 // Jinv is the inverse inertia matrix
 332 // vMoments is the moment vector in the body frame
 333 // VState.vPQRi is the total inertial angular velocity of the vehicle
 334 // expressed in the body frame.
 335 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 336 //            Second edition (2004), eqn 1.5-16e (page 50)
 337
 338 void FGPropagate::CalculatePQRdot(void)
 339 {
 340   const FGColumnVector3& vMoments = Aircraft->GetMoments(); // current moments
 341   const FGMatrix33& J = MassBalance->GetJ();                // inertia matrix
 342   const FGMatrix33& Jinv = MassBalance->GetJinv();          // inertia matrix inverse
 343
 344   // Compute body frame rotational accelerations based on the current body
 345   // moments and the total inertial angular velocity expressed in the body
 346   // frame.
 347
 348   vPQRdot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
 349 }
 350
 351 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 352 // Compute the quaternion orientation derivative
 353 //
 354 // vQtrndot is the quaternion derivative.
 355 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 356 //            Second edition (2004), eqn 1.5-16b (page 50)
 357
 358 void FGPropagate::CalculateQuatdot(void)
 359 {
 360   // Compute quaternion orientation derivative on current body rates
 361   vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi);
 362 }
 363
 364 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 365 // This set of calculations results in the body frame accelerations being
 366 // computed.
 367 // Compute body frame accelerations based on the current body forces.
 368 // Include centripetal and coriolis accelerations.
 369 // vOmegaEarth is the Earth angular rate - expressed in the inertial frame -
 370 //   so it has to be transformed to the body frame. More completely,
 371 //   vOmegaEarth is the rate of the ECEF frame relative to the Inertial
 372 //   frame (ECI), expressed in the Inertial frame.
 373 // vForces is the total force on the vehicle in the body frame.
 374 // VState.vPQR is the vehicle body rate relative to the ECEF frame, expressed
 375 //   in the body frame.
 376 // VState.vUVW is the vehicle velocity relative to the ECEF frame, expressed
 377 //   in the body frame.
 378 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 379 //            Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
 380
 381 void FGPropagate::CalculateUVWdot(void)
 382 {
 383   double mass = MassBalance->GetMass();                      // mass
 384   const FGColumnVector3& vForces = Aircraft->GetForces();    // current forces
 385
 386   vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
 387
 388   // Include Centripetal acceleration.
 389   vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
 390
 391   // Include Gravitation accel
 392   switch (gravType) {
 393     case gtStandard:
 394       vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) );
 395       break;
 396     case gtWGS84:
 397       vGravAccel = Tec2b * Inertial->GetGravityJ2(VState.vLocation);
 398       break;
 399   }
 400
 401   vUVWdot += vGravAccel;
 402 }
 403
 404 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 405   // Transform the velocity vector of the body relative to the origin (Earth
 406   // center) to be expressed in the inertial frame, and add the vehicle velocity
 407   // contribution due to the rotation of the planet.
 408   // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 409   //            Second edition (2004), eqn 1.5-16c (page 50)
 410
 411 void FGPropagate::CalculateInertialVelocity(void)
 412 {
 413   VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
 414 }
 415
 416 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 417
 418 void FGPropagate::Integrate( FGColumnVector3& Integrand,
 419                              FGColumnVector3& Val,
 420                              deque <FGColumnVector3>& ValDot,
 421                              double dt,
 422                              eIntegrateType integration_type)
 423 {
 424   ValDot.push_front(Val);
 425   ValDot.pop_back();
 426
 427   switch(integration_type) {
 428   case eRectEuler:       Integrand += dt*ValDot[0];
 429     break;
 430   case eTrapezoidal:     Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
 431     break;
 432   case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
 433     break;
 434   case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
 435     break;
 436   case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
 437     break;
 438   case eNone: // do nothing, freeze translational rate
 439     break;
 440   }
 441 }
 442
 443 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 444
 445 void FGPropagate::Integrate( FGQuaternion& Integrand,
 446                              FGQuaternion& Val,
 447                              deque <FGQuaternion>& ValDot,
 448                              double dt,
 449                              eIntegrateType integration_type)
 450 {
 451   ValDot.push_front(Val);
 452   ValDot.pop_back();
 453
 454   switch(integration_type) {
 455   case eRectEuler:       Integrand += dt*ValDot[0];
 456     break;
 457   case eTrapezoidal:     Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
 458     break;
 459   case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
 460     break;
 461   case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
 462     break;
 463   case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
 464     break;
 465   case eNone: // do nothing, freeze rotational rate
 466     break;
 467   }
 468 }
 469
 470 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 471 // Resolves the contact forces just before integrating the EOM.
 472 // This routine is using Lagrange multipliers and the projected Gauss-Seidel
 473 // (PGS) method.
 474 // Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence",
 475 //            February 22, 2005
 476 // In JSBSim there is only one rigid body (the aircraft) and there can be
 477 // multiple points of contact between the aircraft and the ground. As a
 478 // consequence our matrix J*M^-1*J^T is not sparse and the algorithm described
 479 // in Catto's paper has been adapted accordingly.
 480
 481 void FGPropagate::ResolveFrictionForces(double dt)
 482 {
 483   const double invMass = 1.0 / MassBalance->GetMass();
 484   const FGMatrix33& Jinv = MassBalance->GetJinv();
 485   vector <FGColumnVector3> JacF, JacM;
 486   FGColumnVector3 vdot, wdot;
 487   FGColumnVector3 Fc, Mc;
 488   int n = 0, i;
 489
 490   // Compiles data from the ground reactions to build up the jacobian matrix
 491   for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, n++) {
 492     JacF.push_back((*it)->ForceJacobian);
 493     JacM.push_back((*it)->MomentJacobian);
 494   }
 495
 496   // If no gears are in contact with the ground then return
 497   if (!n) return;
 498
 499   vector<double> a(n*n); // Will contain J*M^-1*J^T
 500   vector<double> eta(n);
 501   vector<double> lambda(n);
 502   vector<double> lambdaMin(n);
 503   vector<double> lambdaMax(n);
 504
 505   // Initializes the Lagrange multipliers
 506   i = 0;
 507   for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, i++) {
 508     lambda[i] = (*it)->value;
 509     lambdaMax[i] = (*it)->Max;
 510     lambdaMin[i] = (*it)->Min;
 511   }
 512
 513   vdot = vUVWdot;
 514   wdot = vPQRdot;
 515
 516   if (dt > 0.) {
 517     // First compute the ground velocity below the aircraft center of gravity
 518     FGLocation contact;
 519     FGColumnVector3 normal, cvel;
 520
 521     // Instruct the algorithm to zero out the relative movement between the
 522     // aircraft and the ground.
 523     vdot += (VState.vUVW - Tec2b * cvel) / dt;
 524     wdot += VState.vPQR / dt;
 525   }
 526
 527   // Assemble the linear system of equations
 528   for (i=0; i < n; i++) {
 529     for (int j=0; j < i; j++)
 530       a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of J^T*M^-1*J
 531     for (int j=i; j < n; j++)
 532       a[i*n+j] = DotProduct(JacF[i],invMass*JacF[j])+DotProduct(JacM[i],Jinv*JacM[j]);
 533   }
 534
 535   // Prepare the linear system for the Gauss-Seidel algorithm :
 536   // divide every line of 'a' and eta by a[i,i]. This is in order to save
 537   // a division computation at each iteration of Gauss-Seidel.
 538   for (i=0; i < n; i++) {
 539     double d = 1.0 / a[i*n+i];
 540
 541     eta[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
 542     for (int j=0; j < n; j++)
 543       a[i*n+j] *= d;
 544   }
 545
 546   // Resolve the Lagrange multipliers with the projected Gauss-Seidel method
 547   for (int iter=0; iter < 50; iter++) {
 548     double norm = 0.;
 549
 550     for (i=0; i < n; i++) {
 551       double lambda0 = lambda[i];
 552       double dlambda = eta[i];
 553
 554       for (int j=0; j < n; j++)
 555         dlambda -= a[i*n+j]*lambda[j];
 556
 557       lambda[i] = Constrain(lambdaMin[i], lambda0+dlambda, lambdaMax[i]);
 558       dlambda = lambda[i] - lambda0;
 559
 560       norm += fabs(dlambda);
 561     }
 562
 563     if (norm < 1E-5) break;
 564   }
 565
 566   // Calculate the total friction forces and moments
 567
 568   Fc.InitMatrix();
 569   Mc.InitMatrix();
 570
 571   for (i=0; i< n; i++) {
 572     Fc += lambda[i]*JacF[i];
 573     Mc += lambda[i]*JacM[i];
 574   }
 575
 576   vUVWdot += invMass * Fc;
 577   vPQRdot += Jinv * Mc;
 578
 579   // Save the value of the Lagrange multipliers to accelerate the convergence
 580   // of the Gauss-Seidel algorithm at next iteration.
 581   i = 0;
 582   for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it)
 583     (*it)->value = lambda[i++];
 584
 585   GroundReactions->UpdateForcesAndMoments();
 586 }
 587
 588 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 589
 590 void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
 591   VState.qAttitudeECI = Qi;
 592 }
 593
 594 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 595
 596 void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
 597   VState.vInertialVelocity = Vi;
 598 }
 599
 600 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 601
 602 void FGPropagate::RecomputeLocalTerrainRadius(void)
 603 {
 604   double t = FDMExec->GetSimTime();
 605
 606   // Get the LocalTerrain radius.
 607   FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
 608   LocalTerrainRadius = contactloc.GetRadius();
 609 }
 610
 611 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 612
 613 void FGPropagate::SetTerrainElevation(double terrainElev)
 614 {
 615   LocalTerrainRadius = terrainElev + SeaLevelRadius;
 616   FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
 617 }
 618
 619 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 620
 621 double FGPropagate::GetTerrainElevation(void) const
 622 {
 623   return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
 624 }
 625
 626 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 627 //Todo: when should this be called - when should the new EPA be used to
 628 // calculate the transformation matrix, so that the matrix is not a step
 629 // ahead of the sim and the associated calculations?
 630 const FGMatrix33& FGPropagate::GetTi2ec(void)
 631 {
 632   return VState.vLocation.GetTi2ec();
 633 }
 634
 635 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 636
 637 const FGMatrix33& FGPropagate::GetTec2i(void)
 638 {
 639   return VState.vLocation.GetTec2i();
 640 }
 641
 642 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 643
 644 void FGPropagate::SetAltitudeASL(double altASL)
 645 {
 646   VState.vLocation.SetRadius( altASL + SeaLevelRadius );
 647 }
 648
 649 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 650
 651 double FGPropagate::GetLocalTerrainRadius(void) const
 652 {
 653   return LocalTerrainRadius;
 654 }
 655
 656 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 657
 658 double FGPropagate::GetDistanceAGL(void) const
 659 {
 660   return VState.vLocation.GetRadius() - LocalTerrainRadius;
 661 }
 662
 663 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 664
 665 void FGPropagate::SetDistanceAGL(double tt)
 666 {
 667   VState.vLocation.SetRadius( tt + LocalTerrainRadius );
 668 }
 669
 670 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 671
 672 void FGPropagate::bind(void)
 673 {
 674   typedef double (FGPropagate::*PMF)(int) const;
 675
 676   PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
 677
 678   PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
 679   PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
 680   PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
 681
 682   PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
 683   PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
 684   PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
 685
 686   PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
 687   PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
 688   PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
 689
 690   PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
 691   PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
 692   PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
 693
 694   PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
 695
 696   PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
 697   PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
 698   PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
 699
 700   PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
 701   PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
 702   PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
 703
 704   PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
 705   PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
 706   PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
 707   PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
 708   PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
 709   PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
 710   PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
 711   PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
 712   PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
 713   PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
 714   PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
 715   PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
 716                           &FGPropagate::GetTerrainElevation,
 717                           &FGPropagate::SetTerrainElevation, false);
 718
 719   PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
 720
 721   PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 722   PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 723   PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 724
 725   PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 726   PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 727   PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 728
 729   PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
 730   PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
 731   PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
 732   PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
 733   PropertyManager->Tie("simulation/gravity-model", &gravType);
 734 }
 735
 736 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 737 //    The bitmasked value choices are as follows:
 738 //    unset: In this case (the default) JSBSim would only print
 739 //       out the normally expected messages, essentially echoing
 740 //       the config files as they are read. If the environment
 741 //       variable is not set, debug_lvl is set to 1 internally
 742 //    0: This requests JSBSim not to output any messages
 743 //       whatsoever.
 744 //    1: This value explicity requests the normal JSBSim
 745 //       startup messages
 746 //    2: This value asks for a message to be printed out when
 747 //       a class is instantiated
 748 //    4: When this value is set, a message is displayed when a
 749 //       FGModel object executes its Run() method
 750 //    8: When this value is set, various runtime state variables
 751 //       are printed out periodically
 752 //    16: When set various parameters are sanity checked and
 753 //       a message is printed out when they go out of bounds
 754
 755 void FGPropagate::Debug(int from)
 756 {
 757   if (debug_lvl <= 0) return;
 758
 759   if (debug_lvl & 1) { // Standard console startup message output
 760     if (from == 0) { // Constructor
 761
 762     }
 763   }
 764   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 765     if (from == 0) cout << "Instantiated: FGPropagate" << endl;
 766     if (from == 1) cout << "Destroyed:    FGPropagate" << endl;
 767   }
 768   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 769   }
 770   if (debug_lvl & 8 && from == 2) { // Runtime state variables
 771     cout << endl << fgblue << highint << left
 772          << "  Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
 773          << reset << endl;
 774     cout << endl;
 775     cout << highint << "  Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
 776                     << Inertial->GetEarthPositionAngleDeg() << endl;
 777     cout << endl;
 778     cout << highint << "  Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
 779     cout << highint << "  Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
 780     cout << highint << "  Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
 781     cout << highint << "  Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
 782     cout << highint << "  Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
 783     cout << highint << "  Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
 784     cout << highint << "  Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
 785     cout << highint << "  Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
 786     cout << endl;
 787     cout << highint << "  Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
 788                     << reset << endl << Tec2b.Dump("\t", "    ") << endl;
 789     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 790                     << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
 791                     << endl << endl;
 792
 793     cout << highint << "  Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
 794                     << reset << endl << Tb2ec.Dump("\t", "    ") << endl;
 795     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 796                     << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
 797                     << endl << endl;
 798
 799     cout << highint << "  Matrix Local to Body (Orientation of Body with respect to Local):"
 800                     << reset << endl << Tl2b.Dump("\t", "    ") << endl;
 801     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 802                     << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
 803                     << endl << endl;
 804
 805     cout << highint << "  Matrix Body to Local (Orientation of Local with respect to Body):"
 806                     << reset << endl << Tb2l.Dump("\t", "    ") << endl;
 807     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 808                     << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
 809                     << endl << endl;
 810
 811     cout << highint << "  Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
 812                     << reset << endl << Tl2ec.Dump("\t", "    ") << endl;
 813     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 814                     << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
 815                     << endl << endl;
 816
 817     cout << highint << "  Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
 818                     << reset << endl << Tec2l.Dump("\t", "    ") << endl;
 819     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 820                     << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
 821                     << endl << endl;
 822
 823     cout << highint << "  Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
 824                     << reset << endl << Tec2i.Dump("\t", "    ") << endl;
 825     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 826                     << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
 827                     << endl << endl;
 828
 829     cout << highint << "  Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
 830                     << reset << endl << Ti2ec.Dump("\t", "    ") << endl;
 831     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 832                     << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
 833                     << endl << endl;
 834
 835     cout << highint << "  Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
 836                     << reset << endl << Ti2b.Dump("\t", "    ") << endl;
 837     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 838                     << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
 839                     << endl << endl;
 840
 841     cout << highint << "  Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
 842                     << reset << endl << Tb2i.Dump("\t", "    ") << endl;
 843     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 844                     << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
 845                     << endl << endl;
 846
 847     cout << highint << "  Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
 848                     << reset << endl << Ti2l.Dump("\t", "    ") << endl;
 849     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 850                     << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
 851                     << endl << endl;
 852
 853     cout << highint << "  Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
 854                     << reset << endl << Tl2i.Dump("\t", "    ") << endl;
 855     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 856                     << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
 857                     << endl << endl;
 858
 859     cout << setprecision(6); // reset the output stream
 860   }
 861   if (debug_lvl & 16) { // Sanity checking
 862     if (from == 2) { // State sanity checking
 863       if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
 864         cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
 865         exit(-1);
 866       }
 867       if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
 868         cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
 869         exit(-1);
 870       }
 871       if (fabs(GetDistanceAGL()) > 1e10) {
 872         cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
 873         exit(-1);
 874       }
 875     }
 876   }
 877   if (debug_lvl & 64) {
 878     if (from == 0) { // Constructor
 879       cout << IdSrc << endl;
 880       cout << IdHdr << endl;
 881     }
 882   }
 883 }
 884 }