src/FDM/JSBSim/models/FGPropagate.cpp

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