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