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
  52 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  53 INCLUDES
  54 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  55
  56 #include <cmath>
  57 #include <cstdlib>
  58 #include <iostream>
  59 #include <iomanip>
  60
  61 #include "FGPropagate.h"
  62 #include "FGGroundReactions.h"
  63 #include "FGFDMExec.h"
  64 #include "FGAircraft.h"
  65 #include "FGMassBalance.h"
  66 #include "FGInertial.h"
  67 #include "input_output/FGPropertyManager.h"
  68
  69 using namespace std;
  70
  71 namespace JSBSim {
  72
  73 static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.55 2010/07/09 04:11:45 jberndt Exp $";
  74 static const char *IdHdr = ID_PROPAGATE;
  75
  76 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  77 CLASS IMPLEMENTATION
  78 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  79
  80 FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
  81 {
  82   Debug(0);
  83   Name = "FGPropagate";
  84   gravType = gtWGS84;
  85
  86   vPQRdot.InitMatrix();
  87   vQtrndot = FGQuaternion(0,0,0);
  88   vUVWdot.InitMatrix();
  89   vInertialVelocity.InitMatrix();
  90
  91   integrator_rotational_rate = eAdamsBashforth2;
  92   integrator_translational_rate = eTrapezoidal;
  93   integrator_rotational_position = eAdamsBashforth2;
  94   integrator_translational_position = eTrapezoidal;
  95
  96   VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
  97   VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
  98   VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
  99   VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0));
 100
 101   bind();
 102   Debug(0);
 103 }
 104
 105 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 106
 107 FGPropagate::~FGPropagate(void)
 108 {
 109   Debug(1);
 110 }
 111
 112 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 113
 114 bool FGPropagate::InitModel(void)
 115 {
 116   if (!FGModel::InitModel()) return false;
 117
 118   // For initialization ONLY:
 119   SeaLevelRadius = LocalTerrainRadius = Inertial->GetRefRadius();
 120
 121   VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
 122   VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
 123   vOmegaEarth = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
 124
 125   vPQRdot.InitMatrix();
 126   vQtrndot = FGQuaternion(0,0,0);
 127   vUVWdot.InitMatrix();
 128   vInertialVelocity.InitMatrix();
 129
 130   VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 131   VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 132   VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
 133   VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 134
 135   integrator_rotational_rate = eAdamsBashforth2;
 136   integrator_translational_rate = eTrapezoidal;
 137   integrator_rotational_position = eAdamsBashforth2;
 138   integrator_translational_position = eTrapezoidal;
 139
 140   return true;
 141 }
 142
 143 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 144
 145 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
 146 {
 147   SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
 148   SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
 149
 150   VehicleRadius = GetRadius();
 151   radInv = 1.0/VehicleRadius;
 152
 153   // Initialize the State Vector elements and the transformation matrices
 154
 155   // Set the position lat/lon/radius
 156   VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
 157                                 FGIC->GetLatitudeRadIC(),
 158                                 FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
 159
 160   VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle());
 161
 162   Ti2ec = GetTi2ec();         // ECI to ECEF transform
 163   Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
 164
 165   Tl2ec = GetTl2ec();         // local to ECEF transform
 166   Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
 167
 168   Ti2l  = GetTi2l();
 169   Tl2i  = Ti2l.Transposed();
 170
 171   // Set the orientation from the euler angles (is normalized within the
 172   // constructor). The Euler angles represent the orientation of the body
 173   // frame relative to the local frame.
 174   VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(),
 175                                         FGIC->GetThetaRadIC(),
 176                                         FGIC->GetPsiRadIC() );
 177
 178   VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
 179
 180   Ti2b  = GetTi2b();           // ECI to body frame transform
 181   Tb2i  = Ti2b.Transposed();   // body to ECI frame transform
 182
 183   Tl2b  = VState.qAttitudeLocal; // local to body frame transform
 184   Tb2l  = Tl2b.Transposed();     // body to local frame transform
 185
 186   Tec2b = Tl2b * Tec2l;        // ECEF to body frame transform
 187   Tb2ec = Tec2b.Transposed();  // body to ECEF frame tranform
 188
 189   // Set the velocities in the instantaneus body frame
 190   VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
 191                                  FGIC->GetVBodyFpsIC(),
 192                                  FGIC->GetWBodyFpsIC() );
 193
 194   VState.vInertialPosition = Tec2i * VState.vLocation;
 195
 196   // Compute the local frame ECEF velocity
 197   vVel = Tb2l * VState.vUVW;
 198
 199   // Refer to Stevens and Lewis, 1.5-14a, pg. 49.
 200   // This is the rotation rate of the "Local" frame, expressed in the local frame.
 201
 202   FGColumnVector3 vOmegaLocal = FGColumnVector3(
 203      radInv*vVel(eEast),
 204     -radInv*vVel(eNorth),
 205     -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
 206
 207   // Set the angular velocities of the body frame relative to the ECEF frame,
 208   // expressed in the body frame. Effectively, this is:
 209   //   w_b/e = w_b/l + w_l/e
 210   VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
 211                                  FGIC->GetQRadpsIC(),
 212                                  FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal;
 213
 214   VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
 215
 216   // Make an initial run and set past values
 217   CalculatePQRdot();           // Angular rate derivative
 218   CalculateUVWdot();           // Translational rate derivative
 219   CalculateQuatdot();          // Angular orientation derivative
 220   CalculateInertialVelocity(); // Translational position derivative
 221
 222   // Initialize past values deques
 223   VState.dqPQRdot.clear();
 224   VState.dqUVWdot.clear();
 225   VState.dqInertialVelocity.clear();
 226   VState.dqQtrndot.clear();
 227   for (int i=0; i<4; i++) {
 228     VState.dqPQRdot.push_front(vPQRdot);
 229     VState.dqUVWdot.push_front(vUVWdot);
 230     VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
 231     VState.dqQtrndot.push_front(vQtrndot);
 232   }
 233
 234   // Recompute the LocalTerrainRadius.
 235   RecomputeLocalTerrainRadius();
 236 }
 237
 238 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 239 /*
 240 Purpose: Called on a schedule to perform EOM integration
 241 Notes:   [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
 242          In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
 243
 244 At the top of this Run() function, see several "shortcuts" (or, aliases) being
 245 set up for use later, rather than using the longer class->function() notation.
 246
 247 This propagation is done using the current state values
 248 and current derivatives. Based on these values we compute an approximation to the
 249 state values for (now + dt).
 250
 251 In the code below, variables named beginning with a small "v" refer to a
 252 a column vector, variables named beginning with a "T" refer to a transformation
 253 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
 254 Inertial.
 255
 256 */
 257
 258 bool FGPropagate::Run(void)
 259 {
 260 static int ctr;
 261
 262   if (FGModel::Run()) return true;  // Fast return if we have nothing to do ...
 263   if (FDMExec->Holding()) return false;
 264
 265   double dt = FDMExec->GetDeltaT()*rate;  // The 'stepsize'
 266
 267   RunPreFunctions();
 268
 269   // Calculate state derivatives
 270   CalculatePQRdot();           // Angular rate derivative
 271   CalculateUVWdot();           // Translational rate derivative
 272   CalculateQuatdot();          // Angular orientation derivative
 273   CalculateInertialVelocity(); // Translational position derivative
 274
 275   // Propagate rotational / translational velocity, angular /translational position, respectively.
 276   Integrate(VState.vPQRi,             vPQRdot,           VState.dqPQRdot,           dt, integrator_rotational_rate);
 277   Integrate(VState.vUVW,              vUVWdot,           VState.dqUVWdot,           dt, integrator_translational_rate);
 278   Integrate(VState.qAttitudeECI,      vQtrndot,          VState.dqQtrndot,          dt, integrator_rotational_position);
 279   Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
 280
 281   VState.qAttitudeECI.Normalize(); // Normalize the ECI Attitude quaternion
 282
 283   VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle()); // Update the Earth position angle (EPA)
 284
 285   // Update the "Location-based" transformation matrices from the vLocation vector.
 286
 287   Ti2ec = GetTi2ec();          // ECI to ECEF transform
 288   Tec2i = Ti2ec.Transposed();  // ECEF to ECI frame transform
 289   Tl2ec = GetTl2ec();          // local to ECEF transform
 290   Tec2l = Tl2ec.Transposed();  // ECEF to local frame transform
 291   Ti2l  = GetTi2l();
 292   Tl2i  = Ti2l.Transposed();
 293
 294   // Update the "Orientation-based" transformation matrices from the orientation quaternion
 295
 296   Ti2b  = GetTi2b();           // ECI to body frame transform
 297   Tb2i  = Ti2b.Transposed();   // body to ECI frame transform
 298   Tl2b  = Ti2b*Tl2i;           // local to body frame transform
 299   Tb2l  = Tl2b.Transposed();   // body to local frame transform
 300   Tec2b = Tl2b * Tec2l;        // ECEF to body frame transform
 301   Tb2ec = Tec2b.Transposed();  // body to ECEF frame tranform
 302
 303   // Set auxililary state variables
 304   VState.vLocation = Ti2ec*VState.vInertialPosition;
 305   RecomputeLocalTerrainRadius();
 306
 307   // Calculate current aircraft radius from center of planet
 308   VehicleRadius = VState.vInertialPosition.Magnitude();
 309   radInv = 1.0/VehicleRadius;
 310
 311   VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
 312
 313   VState.qAttitudeLocal = Tl2b.GetQuaternion();
 314
 315   // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
 316   vVel = Tb2l * VState.vUVW;
 317
 318   RunPostFunctions();
 319
 320   Debug(2);
 321   return false;
 322 }
 323
 324 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 325 // Compute body frame rotational accelerations based on the current body moments
 326 //
 327 // vPQRdot is the derivative of the absolute angular velocity of the vehicle
 328 // (body rate with respect to the inertial frame), expressed in the body frame,
 329 // where the derivative is taken in the body frame.
 330 // J is the inertia matrix
 331 // Jinv is the inverse inertia matrix
 332 // vMoments is the moment vector in the body frame
 333 // VState.vPQRi is the total inertial angular velocity of the vehicle
 334 // expressed in the body frame.
 335 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 336 //            Second edition (2004), eqn 1.5-16e (page 50)
 337
 338 void FGPropagate::CalculatePQRdot(void)
 339 {
 340   const FGColumnVector3& vMoments = Aircraft->GetMoments(); // current moments
 341   const FGMatrix33& J = MassBalance->GetJ();                // inertia matrix
 342   const FGMatrix33& Jinv = MassBalance->GetJinv();          // inertia matrix inverse
 343
 344   // Compute body frame rotational accelerations based on the current body
 345   // moments and the total inertial angular velocity expressed in the body
 346   // frame.
 347
 348   vPQRdot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
 349 }
 350
 351 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 352 // Compute the quaternion orientation derivative
 353 //
 354 // vQtrndot is the quaternion derivative.
 355 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 356 //            Second edition (2004), eqn 1.5-16b (page 50)
 357
 358 void FGPropagate::CalculateQuatdot(void)
 359 {
 360   // Compute quaternion orientation derivative on current body rates
 361   vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi);
 362 }
 363
 364 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 365 // This set of calculations results in the body frame accelerations being
 366 // computed.
 367 // Compute body frame accelerations based on the current body forces.
 368 // Include centripetal and coriolis accelerations.
 369 // vOmegaEarth is the Earth angular rate - expressed in the inertial frame -
 370 //   so it has to be transformed to the body frame. More completely,
 371 //   vOmegaEarth is the rate of the ECEF frame relative to the Inertial
 372 //   frame (ECI), expressed in the Inertial frame.
 373 // vForces is the total force on the vehicle in the body frame.
 374 // VState.vPQR is the vehicle body rate relative to the ECEF frame, expressed
 375 //   in the body frame.
 376 // VState.vUVW is the vehicle velocity relative to the ECEF frame, expressed
 377 //   in the body frame.
 378 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 379 //            Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
 380
 381 void FGPropagate::CalculateUVWdot(void)
 382 {
 383   double mass = MassBalance->GetMass();                      // mass
 384   const FGColumnVector3& vForces = Aircraft->GetForces();    // current forces
 385
 386   vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
 387
 388   // Include Centripetal acceleration.
 389   if (!GroundReactions->GetWOW() && Aircraft->GetHoldDown() == 0) {
 390     vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
 391   }
 392
 393   // Include Gravitation accel
 394   switch (gravType) {
 395     case gtStandard:
 396       vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) );
 397       break;
 398     case gtWGS84:
 399       vGravAccel = Tec2b * Inertial->GetGravityJ2(VState.vLocation);
 400       break;
 401   }
 402
 403   vUVWdot += vGravAccel;
 404 }
 405
 406 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 407   // Transform the velocity vector of the body relative to the origin (Earth
 408   // center) to be expressed in the inertial frame, and add the vehicle velocity
 409   // contribution due to the rotation of the planet.
 410   // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 411   //            Second edition (2004), eqn 1.5-16c (page 50)
 412
 413 void FGPropagate::CalculateInertialVelocity(void)
 414 {
 415   VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
 416 }
 417
 418 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 419
 420 void FGPropagate::Integrate( FGColumnVector3& Integrand,
 421                              FGColumnVector3& Val,
 422                              deque <FGColumnVector3>& ValDot,
 423                              double dt,
 424                              eIntegrateType integration_type)
 425 {
 426   ValDot.push_front(Val);
 427   ValDot.pop_back();
 428
 429   switch(integration_type) {
 430   case eRectEuler:       Integrand += dt*ValDot[0];
 431     break;
 432   case eTrapezoidal:     Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
 433     break;
 434   case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
 435     break;
 436   case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
 437     break;
 438   case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
 439     break;
 440   case eNone: // do nothing, freeze translational rate
 441     break;
 442   }
 443 }
 444
 445 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 446
 447 void FGPropagate::Integrate( FGQuaternion& Integrand,
 448                              FGQuaternion& Val,
 449                              deque <FGQuaternion>& ValDot,
 450                              double dt,
 451                              eIntegrateType integration_type)
 452 {
 453   ValDot.push_front(Val);
 454   ValDot.pop_back();
 455
 456   switch(integration_type) {
 457   case eRectEuler:       Integrand += dt*ValDot[0];
 458     break;
 459   case eTrapezoidal:     Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
 460     break;
 461   case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
 462     break;
 463   case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
 464     break;
 465   case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
 466     break;
 467   case eNone: // do nothing, freeze translational rate
 468     break;
 469   }
 470 }
 471
 472 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 473
 474 void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
 475   VState.qAttitudeECI = Qi;
 476 }
 477
 478 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 479
 480 void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
 481   VState.vInertialVelocity = Vi;
 482 }
 483
 484 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 485
 486 void FGPropagate::RecomputeLocalTerrainRadius(void)
 487 {
 488   double t = FDMExec->GetSimTime();
 489
 490   // Get the LocalTerrain radius.
 491 //  FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
 492 //  LocalTerrainRadius = contactloc.GetRadius();
 493   LocalTerrainRadius = FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius();
 494 }
 495
 496 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 497
 498 void FGPropagate::SetTerrainElevation(double terrainElev)
 499 {
 500   LocalTerrainRadius = terrainElev + SeaLevelRadius;
 501   FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
 502 }
 503
 504 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 505
 506 double FGPropagate::GetTerrainElevation(void) const
 507 {
 508   return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
 509 }
 510
 511 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 512 //Todo: when should this be called - when should the new EPA be used to
 513 // calculate the transformation matrix, so that the matrix is not a step
 514 // ahead of the sim and the associated calculations?
 515 const FGMatrix33& FGPropagate::GetTi2ec(void)
 516 {
 517   return VState.vLocation.GetTi2ec();
 518 }
 519
 520 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 521
 522 const FGMatrix33& FGPropagate::GetTec2i(void)
 523 {
 524   return VState.vLocation.GetTec2i();
 525 }
 526
 527 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 528
 529 void FGPropagate::SetAltitudeASL(double altASL)
 530 {
 531   VState.vLocation.SetRadius( altASL + SeaLevelRadius );
 532 }
 533
 534 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 535
 536 double FGPropagate::GetLocalTerrainRadius(void) const
 537 {
 538   return LocalTerrainRadius;
 539 }
 540
 541 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 542
 543 double FGPropagate::GetDistanceAGL(void) const
 544 {
 545   return VState.vLocation.GetRadius() - LocalTerrainRadius;
 546 }
 547
 548 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 549
 550 void FGPropagate::SetDistanceAGL(double tt)
 551 {
 552   VState.vLocation.SetRadius( tt + LocalTerrainRadius );
 553 }
 554
 555 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 556
 557 void FGPropagate::bind(void)
 558 {
 559   typedef double (FGPropagate::*PMF)(int) const;
 560
 561   PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
 562
 563   PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
 564   PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
 565   PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
 566
 567   PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
 568   PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
 569   PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
 570
 571   PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
 572   PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
 573   PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
 574
 575   PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
 576   PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
 577   PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
 578
 579   PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
 580
 581   PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
 582   PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
 583   PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
 584
 585   PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
 586   PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
 587   PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
 588
 589   PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
 590   PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
 591   PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
 592   PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
 593   PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
 594   PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
 595   PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
 596   PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
 597   PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
 598   PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
 599   PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
 600   PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
 601                           &FGPropagate::GetTerrainElevation,
 602                           &FGPropagate::SetTerrainElevation, false);
 603
 604   PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
 605
 606   PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 607   PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 608   PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 609
 610   PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 611   PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 612   PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 613
 614   PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
 615   PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
 616   PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
 617   PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
 618   PropertyManager->Tie("simulation/gravity-model", &gravType);
 619 }
 620
 621 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 622 //    The bitmasked value choices are as follows:
 623 //    unset: In this case (the default) JSBSim would only print
 624 //       out the normally expected messages, essentially echoing
 625 //       the config files as they are read. If the environment
 626 //       variable is not set, debug_lvl is set to 1 internally
 627 //    0: This requests JSBSim not to output any messages
 628 //       whatsoever.
 629 //    1: This value explicity requests the normal JSBSim
 630 //       startup messages
 631 //    2: This value asks for a message to be printed out when
 632 //       a class is instantiated
 633 //    4: When this value is set, a message is displayed when a
 634 //       FGModel object executes its Run() method
 635 //    8: When this value is set, various runtime state variables
 636 //       are printed out periodically
 637 //    16: When set various parameters are sanity checked and
 638 //       a message is printed out when they go out of bounds
 639
 640 void FGPropagate::Debug(int from)
 641 {
 642   if (debug_lvl <= 0) return;
 643
 644   if (debug_lvl & 1) { // Standard console startup message output
 645     if (from == 0) { // Constructor
 646
 647     }
 648   }
 649   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 650     if (from == 0) cout << "Instantiated: FGPropagate" << endl;
 651     if (from == 1) cout << "Destroyed:    FGPropagate" << endl;
 652   }
 653   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 654   }
 655   if (debug_lvl & 8 && from == 2) { // Runtime state variables
 656     cout << endl << fgblue << highint << left
 657          << "  Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
 658          << reset << endl;
 659     cout << endl;
 660     cout << highint << "  Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
 661                     << Inertial->GetEarthPositionAngleDeg() << endl;
 662     cout << endl;
 663     cout << highint << "  Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
 664     cout << highint << "  Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
 665     cout << highint << "  Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
 666     cout << highint << "  Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
 667     cout << highint << "  Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
 668     cout << highint << "  Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
 669     cout << highint << "  Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
 670     cout << highint << "  Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
 671     cout << endl;
 672     cout << highint << "  Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
 673                     << reset << endl << Tec2b.Dump("\t", "    ") << endl;
 674     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 675                     << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
 676                     << endl << endl;
 677
 678     cout << highint << "  Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
 679                     << reset << endl << Tb2ec.Dump("\t", "    ") << endl;
 680     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 681                     << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
 682                     << endl << endl;
 683
 684     cout << highint << "  Matrix Local to Body (Orientation of Body with respect to Local):"
 685                     << reset << endl << Tl2b.Dump("\t", "    ") << endl;
 686     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 687                     << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
 688                     << endl << endl;
 689
 690     cout << highint << "  Matrix Body to Local (Orientation of Local with respect to Body):"
 691                     << reset << endl << Tb2l.Dump("\t", "    ") << endl;
 692     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 693                     << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
 694                     << endl << endl;
 695
 696     cout << highint << "  Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
 697                     << reset << endl << Tl2ec.Dump("\t", "    ") << endl;
 698     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 699                     << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
 700                     << endl << endl;
 701
 702     cout << highint << "  Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
 703                     << reset << endl << Tec2l.Dump("\t", "    ") << endl;
 704     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 705                     << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
 706                     << endl << endl;
 707
 708     cout << highint << "  Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
 709                     << reset << endl << Tec2i.Dump("\t", "    ") << endl;
 710     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 711                     << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
 712                     << endl << endl;
 713
 714     cout << highint << "  Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
 715                     << reset << endl << Ti2ec.Dump("\t", "    ") << endl;
 716     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 717                     << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
 718                     << endl << endl;
 719
 720     cout << highint << "  Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
 721                     << reset << endl << Ti2b.Dump("\t", "    ") << endl;
 722     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 723                     << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
 724                     << endl << endl;
 725
 726     cout << highint << "  Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
 727                     << reset << endl << Tb2i.Dump("\t", "    ") << endl;
 728     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 729                     << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
 730                     << endl << endl;
 731
 732     cout << highint << "  Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
 733                     << reset << endl << Ti2l.Dump("\t", "    ") << endl;
 734     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 735                     << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
 736                     << endl << endl;
 737
 738     cout << highint << "  Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
 739                     << reset << endl << Tl2i.Dump("\t", "    ") << endl;
 740     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 741                     << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
 742                     << endl << endl;
 743
 744     cout << setprecision(6); // reset the output stream
 745   }
 746   if (debug_lvl & 16) { // Sanity checking
 747     if (from == 2) { // State sanity checking
 748       if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
 749         cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
 750         exit(-1);
 751       }
 752       if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
 753         cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
 754         exit(-1);
 755       }
 756       if (fabs(GetDistanceAGL()) > 1e10) {
 757         cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
 758         exit(-1);
 759       }
 760     }
 761   }
 762   if (debug_lvl & 64) {
 763     if (from == 0) { // Constructor
 764       cout << IdSrc << endl;
 765       cout << IdHdr << endl;
 766     }
 767   }
 768 }
 769 }