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