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.76 2011/01/16 16:10:59 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 = VState.vLocation.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 = VState.vLocation.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 'rhs' 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 = VState.vLocation.GetTl2ec(); // local to ECEF transform
 623   Tec2l = Tl2ec.Transposed();          // ECEF to local frame transform
 624   Ti2l  = VState.vLocation.GetTi2l();  // ECI to local frame transform
 625   Tl2i  = Ti2l.Transposed();           // local to ECI transform
 626 }
 627
 628 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 629
 630 void FGPropagate::UpdateBodyMatrices(void)
 631 {
 632   Ti2b  = VState.qAttitudeECI.GetT(); // 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 = Tb2l * 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
 720 double FGPropagate::GetDistanceAGL(void) const
 721 {
 722   return VState.vLocation.GetRadius() - LocalTerrainRadius;
 723 }
 724
 725 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 726
 727 void FGPropagate::SetVState(const VehicleState& vstate)
 728 {
 729   VState.vLocation = vstate.vLocation;
 730   VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
 731   Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
 732   Tec2i = Ti2ec.Transposed();
 733   UpdateLocationMatrices();
 734   SetInertialOrientation(vstate.qAttitudeECI);
 735   RecomputeLocalTerrainRadius();
 736   VehicleRadius = GetRadius();
 737   VState.vUVW = vstate.vUVW;
 738   vVel = Tb2l * VState.vUVW;
 739   VState.vPQR = vstate.vPQR;
 740   VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
 741   VState.vPQRi_i = Tb2i * VState.vPQRi;
 742   VState.vInertialPosition = vstate.vInertialPosition;
 743
 744   InitializeDerivatives();
 745 }
 746
 747 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 748
 749 void FGPropagate::UpdateVehicleState(void)
 750 {
 751   RecomputeLocalTerrainRadius();
 752   VehicleRadius = GetRadius();
 753   VState.vInertialPosition = Tec2i * VState.vLocation;
 754   UpdateLocationMatrices();
 755   UpdateBodyMatrices();
 756   vVel = Tb2l * VState.vUVW;
 757   VState.qAttitudeLocal = Tl2b.GetQuaternion();
 758 }
 759
 760 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 761
 762 void FGPropagate::SetLocation(const FGLocation& l)
 763 {
 764   VState.vLocation = l;
 765   VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
 766   Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
 767   Tec2i = Ti2ec.Transposed();
 768   UpdateVehicleState();
 769 }
 770
 771 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 772
 773 void FGPropagate::DumpState(void)
 774 {
 775   cout << endl;
 776   cout << fgblue
 777        << "------------------------------------------------------------------" << reset << endl;
 778   cout << highint
 779        << "State Report at sim time: " << FDMExec->GetSimTime() << " seconds" << reset << endl;
 780   cout << "  " << underon
 781        <<   "Position" << underoff << endl;
 782   cout << "    ECI:   " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl;
 783   cout << "    ECEF:  " << VState.vLocation << " (x,y,z, in ft)"  << endl;
 784   cout << "    Local: " << VState.vLocation.GetLatitudeDeg()
 785                         << ", " << VState.vLocation.GetLongitudeDeg()
 786                         << ", " << GetAltitudeASL() << " (lat, lon, alt in deg and ft)" << endl;
 787
 788   cout << endl << "  " << underon
 789        <<   "Orientation" << underoff << endl;
 790   cout << "    ECI:   " << VState.qAttitudeECI.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
 791   cout << "    Local: " << VState.qAttitudeLocal.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
 792
 793   cout << endl << "  " << underon
 794        <<   "Velocity" << underoff << endl;
 795   cout << "    ECI:   " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl;
 796   cout << "    ECEF:  " << (Tb2ec * VState.vUVW).Dump(", ")  << " (x,y,z in ft/s)"  << endl;
 797   cout << "    Local: " << GetVel() << " (n,e,d in ft/sec)" << endl;
 798   cout << "    Body:  " << GetUVW() << " (u,v,w in ft/sec)" << endl;
 799
 800   cout << endl << "  " << underon
 801        <<   "Body Rates (relative to given frame, expressed in body frame)" << underoff << endl;
 802   cout << "    ECI:   " << (VState.vPQRi*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
 803   cout << "    ECEF:  " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
 804 }
 805
 806 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 807
 808 void FGPropagate::bind(void)
 809 {
 810   typedef double (FGPropagate::*PMF)(int) const;
 811
 812   PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
 813
 814   PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
 815   PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
 816   PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
 817
 818   PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
 819   PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
 820   PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
 821
 822   PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
 823   PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
 824   PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
 825
 826   PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
 827   PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
 828   PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
 829
 830   PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
 831
 832   PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
 833   PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
 834   PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
 835
 836   PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
 837   PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
 838   PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
 839
 840   PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
 841   PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
 842   PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
 843   PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
 844   PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
 845   PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
 846   PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
 847   PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
 848   PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
 849   PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
 850   PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
 851   PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
 852                           &FGPropagate::GetTerrainElevation,
 853                           &FGPropagate::SetTerrainElevation, false);
 854
 855   PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
 856
 857   PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 858   PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 859   PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 860
 861   PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 862   PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 863   PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 864
 865   PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
 866   PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
 867   PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
 868   PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
 869   PropertyManager->Tie("simulation/gravity-model", &gravType);
 870 }
 871
 872 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 873 //    The bitmasked value choices are as follows:
 874 //    unset: In this case (the default) JSBSim would only print
 875 //       out the normally expected messages, essentially echoing
 876 //       the config files as they are read. If the environment
 877 //       variable is not set, debug_lvl is set to 1 internally
 878 //    0: This requests JSBSim not to output any messages
 879 //       whatsoever.
 880 //    1: This value explicity requests the normal JSBSim
 881 //       startup messages
 882 //    2: This value asks for a message to be printed out when
 883 //       a class is instantiated
 884 //    4: When this value is set, a message is displayed when a
 885 //       FGModel object executes its Run() method
 886 //    8: When this value is set, various runtime state variables
 887 //       are printed out periodically
 888 //    16: When set various parameters are sanity checked and
 889 //       a message is printed out when they go out of bounds
 890
 891 void FGPropagate::Debug(int from)
 892 {
 893   if (debug_lvl <= 0) return;
 894
 895   if (debug_lvl & 1) { // Standard console startup message output
 896     if (from == 0) { // Constructor
 897
 898     }
 899   }
 900   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 901     if (from == 0) cout << "Instantiated: FGPropagate" << endl;
 902     if (from == 1) cout << "Destroyed:    FGPropagate" << endl;
 903   }
 904   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 905   }
 906   if (debug_lvl & 8 && from == 2) { // Runtime state variables
 907     cout << endl << fgblue << highint << left
 908          << "  Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
 909          << reset << endl;
 910     cout << endl;
 911     cout << highint << "  Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
 912                     << FDMExec->GetInertial()->GetEarthPositionAngleDeg() << endl;
 913     cout << endl;
 914     cout << highint << "  Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
 915     cout << highint << "  Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
 916     cout << highint << "  Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
 917     cout << highint << "  Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
 918     cout << highint << "  Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
 919     cout << highint << "  Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
 920     cout << highint << "  Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
 921     cout << highint << "  Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
 922     cout << endl;
 923     cout << highint << "  Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
 924                     << reset << endl << Tec2b.Dump("\t", "    ") << endl;
 925     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 926                     << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
 927                     << endl << endl;
 928
 929     cout << highint << "  Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
 930                     << reset << endl << Tb2ec.Dump("\t", "    ") << endl;
 931     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 932                     << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
 933                     << endl << endl;
 934
 935     cout << highint << "  Matrix Local to Body (Orientation of Body with respect to Local):"
 936                     << reset << endl << Tl2b.Dump("\t", "    ") << endl;
 937     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 938                     << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
 939                     << endl << endl;
 940
 941     cout << highint << "  Matrix Body to Local (Orientation of Local with respect to Body):"
 942                     << reset << endl << Tb2l.Dump("\t", "    ") << endl;
 943     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 944                     << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
 945                     << endl << endl;
 946
 947     cout << highint << "  Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
 948                     << reset << endl << Tl2ec.Dump("\t", "    ") << endl;
 949     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 950                     << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
 951                     << endl << endl;
 952
 953     cout << highint << "  Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
 954                     << reset << endl << Tec2l.Dump("\t", "    ") << endl;
 955     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 956                     << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
 957                     << endl << endl;
 958
 959     cout << highint << "  Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
 960                     << reset << endl << Tec2i.Dump("\t", "    ") << endl;
 961     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 962                     << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
 963                     << endl << endl;
 964
 965     cout << highint << "  Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
 966                     << reset << endl << Ti2ec.Dump("\t", "    ") << endl;
 967     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 968                     << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
 969                     << endl << endl;
 970
 971     cout << highint << "  Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
 972                     << reset << endl << Ti2b.Dump("\t", "    ") << endl;
 973     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 974                     << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
 975                     << endl << endl;
 976
 977     cout << highint << "  Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
 978                     << reset << endl << Tb2i.Dump("\t", "    ") << endl;
 979     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 980                     << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
 981                     << endl << endl;
 982
 983     cout << highint << "  Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
 984                     << reset << endl << Ti2l.Dump("\t", "    ") << endl;
 985     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 986                     << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
 987                     << endl << endl;
 988
 989     cout << highint << "  Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
 990                     << reset << endl << Tl2i.Dump("\t", "    ") << endl;
 991     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 992                     << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
 993                     << endl << endl;
 994
 995     cout << setprecision(6); // reset the output stream
 996   }
 997   if (debug_lvl & 16) { // Sanity checking
 998     if (from == 2) { // State sanity checking
 999       if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
1000         cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
1001         exit(-1);
1002       }
1003       if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
1004         cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
1005         exit(-1);
1006       }
1007       if (fabs(GetDistanceAGL()) > 1e10) {
1008         cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
1009         exit(-1);
1010       }
1011     }
1012   }
1013   if (debug_lvl & 64) {
1014     if (from == 0) { // Constructor
1015       cout << IdSrc << endl;
1016       cout << IdHdr << endl;
1017     }
1018   }
1019 }
1020 }