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