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