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