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