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