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
  60 #include "FGPropagate.h"
  61 #include "FGFDMExec.h"
  62 #include "FGState.h"
  63 #include "FGAircraft.h"
  64 #include "FGMassBalance.h"
  65 #include "FGInertial.h"
  66 #include "input_output/FGPropertyManager.h"
  67
  68 using namespace std;
  69
  70 namespace JSBSim {
  71
  72 static const char *IdSrc = "$Id$";
  73 static const char *IdHdr = ID_PROPAGATE;
  74
  75 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  76 CLASS IMPLEMENTATION
  77 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  78
  79 FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
  80 {
  81   Debug(0);
  82   Name = "FGPropagate";
  83
  84   last2_vPQRdot.InitMatrix();
  85   last_vPQRdot.InitMatrix();
  86   vPQRdot.InitMatrix();
  87
  88   last2_vQtrndot = FGQuaternion(0,0,0);
  89   last_vQtrndot = FGQuaternion(0,0,0);
  90   vQtrndot = FGQuaternion(0,0,0);
  91
  92   last2_vUVWdot.InitMatrix();
  93   last_vUVWdot.InitMatrix();
  94   vUVWdot.InitMatrix();
  95
  96   last2_vLocationDot.InitMatrix();
  97   last_vLocationDot.InitMatrix();
  98   vLocationDot.InitMatrix();
  99
 100   vOmegaLocal.InitMatrix();
 101
 102   integrator_rotational_rate = eAdamsBashforth2;
 103   integrator_translational_rate = eTrapezoidal;
 104   integrator_rotational_position = eAdamsBashforth2;
 105   integrator_translational_position = eTrapezoidal;
 106
 107   bind();
 108   Debug(0);
 109 }
 110
 111 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 112
 113 FGPropagate::~FGPropagate(void)
 114 {
 115   Debug(1);
 116 }
 117
 118 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 119
 120 bool FGPropagate::InitModel(void)
 121 {
 122   if (!FGModel::InitModel()) return false;
 123
 124   // For initialization ONLY:
 125   SeaLevelRadius = LocalTerrainRadius = Inertial->GetRefRadius();
 126
 127   VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
 128   VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
 129   vOmega = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
 130
 131   last2_vPQRdot.InitMatrix();
 132   last_vPQRdot.InitMatrix();
 133   vPQRdot.InitMatrix();
 134
 135   last2_vQtrndot = FGQuaternion(0,0,0);
 136   last_vQtrndot = FGQuaternion(0,0,0);
 137   vQtrndot = FGQuaternion(0,0,0);
 138
 139   last2_vUVWdot.InitMatrix();
 140   last_vUVWdot.InitMatrix();
 141   vUVWdot.InitMatrix();
 142
 143   last2_vLocationDot.InitMatrix();
 144   last_vLocationDot.InitMatrix();
 145   vLocationDot.InitMatrix();
 146
 147   vOmegaLocal.InitMatrix();
 148
 149   integrator_rotational_rate = eAdamsBashforth2;
 150   integrator_translational_rate = eTrapezoidal;
 151   integrator_rotational_position = eAdamsBashforth2;
 152   integrator_translational_position = eTrapezoidal;
 153
 154   return true;
 155 }
 156
 157 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 158
 159 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
 160 {
 161   SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
 162   SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
 163
 164   // Set the position lat/lon/radius
 165   VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
 166                           FGIC->GetLatitudeRadIC(),
 167                           FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
 168
 169   VehicleRadius = GetRadius();
 170   radInv = 1.0/VehicleRadius;
 171
 172   // Set the Orientation from the euler angles
 173   VState.vQtrn = FGQuaternion( FGIC->GetPhiRadIC(),
 174                         FGIC->GetThetaRadIC(),
 175                         FGIC->GetPsiRadIC() );
 176
 177   // Set the velocities in the instantaneus body frame
 178   VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
 179                           FGIC->GetVBodyFpsIC(),
 180                           FGIC->GetWBodyFpsIC() );
 181
 182   // Set the angular velocities in the instantaneus body frame.
 183   VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
 184                           FGIC->GetQRadpsIC(),
 185                           FGIC->GetRRadpsIC() );
 186
 187   // Compute the local frame ECEF velocity
 188   vVel = GetTb2l()*VState.vUVW;
 189
 190   // Finally, make sure that the quaternion stays normalized.
 191   VState.vQtrn.Normalize();
 192
 193   // Recompute the LocalTerrainRadius.
 194   RecomputeLocalTerrainRadius();
 195
 196   // These local copies of the transformation matrices are for use for
 197   // initial conditions only.
 198
 199   Tl2b = GetTl2b();           // local to body frame transform
 200   Tb2l = Tl2b.Transposed();   // body to local frame transform
 201   Tl2ec = GetTl2ec();         // local to ECEF transform
 202   Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
 203   Tec2b = Tl2b * Tec2l;       // ECEF to body frame transform
 204   Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
 205   Ti2ec = GetTi2ec();         // ECI to ECEF transform
 206   Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
 207   Ti2b  = Tec2b*Ti2ec;        // ECI to body frame transform
 208   Tb2i  = Ti2b.Transposed();  // body to ECI frame transform
 209 }
 210
 211 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 212 /*
 213 Purpose: Called on a schedule to perform EOM integration
 214 Notes:   [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
 215          In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
 216
 217 At the top of this Run() function, see several "shortcuts" (or, aliases) being
 218 set up for use later, rather than using the longer class->function() notation.
 219
 220 This propagation is done using the current state values
 221 and current derivatives. Based on these values we compute an approximation to the
 222 state values for (now + dt).
 223
 224 In the code below, variables named beginning with a small "v" refer to a
 225 a column vector, variables named beginning with a "T" refer to a transformation
 226 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
 227 Inertial.
 228
 229 */
 230
 231 bool FGPropagate::Run(void)
 232 {
 233   if (FGModel::Run()) return true;  // Fast return if we have nothing to do ...
 234   if (FDMExec->Holding()) return false;
 235
 236   RecomputeLocalTerrainRadius();
 237
 238   // Calculate current aircraft radius from center of planet
 239
 240   VehicleRadius = GetRadius();
 241   radInv = 1.0/VehicleRadius;
 242
 243   // These local copies of the transformation matrices are for use this
 244   // pass through Run() only.
 245
 246   Tl2b = GetTl2b();           // local to body frame transform
 247   Tb2l = Tl2b.Transposed();   // body to local frame transform
 248   Tl2ec = GetTl2ec();         // local to ECEF transform
 249   Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
 250   Tec2b = Tl2b * Tec2l;       // ECEF to body frame transform
 251   Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
 252   Ti2ec = GetTi2ec();         // ECI to ECEF transform
 253   Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
 254   Ti2b  = Tec2b*Ti2ec;        // ECI to body frame transform
 255   Tb2i  = Ti2b.Transposed();  // body to ECI frame transform
 256
 257   // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
 258   vVel = Tb2l * VState.vUVW;
 259
 260   // Inertial angular velocity measured in the body frame.
 261   vPQRi = VState.vPQR + Tec2b*vOmega;
 262
 263   // Calculate state derivatives
 264   CalculatePQRdot();      // Angular rate derivative
 265   CalculateUVWdot();      // Translational rate derivative
 266   CalculateQuatdot();     // Angular orientation derivative
 267   CalculateLocationdot(); // Translational position derivative
 268
 269   // Integrate to propagate the state
 270
 271   double dt = State->Getdt()*rate;  // The 'stepsize'
 272
 273   // Propagate rotational velocity
 274
 275   switch(integrator_rotational_rate) {
 276   case eRectEuler:       VState.vPQR += dt*vPQRdot;
 277     break;
 278   case eTrapezoidal:     VState.vPQR += 0.5*dt*(vPQRdot + last_vPQRdot);
 279     break;
 280   case eAdamsBashforth2: VState.vPQR += dt*(1.5*vPQRdot - 0.5*last_vPQRdot);
 281     break;
 282   case eAdamsBashforth3: VState.vPQR += (1/12.0)*dt*(23.0*vPQRdot - 16.0*last_vPQRdot + 5.0*last2_vPQRdot);
 283     break;
 284   case eNone: // do nothing, freeze angular rate
 285     break;
 286   }
 287
 288   // Propagate translational velocity
 289
 290   switch(integrator_translational_rate) {
 291   case eRectEuler:       VState.vUVW += dt*vUVWdot;
 292     break;
 293   case eTrapezoidal:     VState.vUVW += 0.5*dt*(vUVWdot + last_vUVWdot);
 294     break;
 295   case eAdamsBashforth2: VState.vUVW += dt*(1.5*vUVWdot - 0.5*last_vUVWdot);
 296     break;
 297   case eAdamsBashforth3: VState.vUVW += (1/12.0)*dt*(23.0*vUVWdot - 16.0*last_vUVWdot + 5.0*last2_vUVWdot);
 298     break;
 299   case eNone: // do nothing, freeze translational rate
 300     break;
 301   }
 302
 303   // Propagate angular position
 304
 305   switch(integrator_rotational_position) {
 306   case eRectEuler:       VState.vQtrn += dt*vQtrndot;
 307     break;
 308   case eTrapezoidal:     VState.vQtrn += 0.5*dt*(vQtrndot + last_vQtrndot);
 309     break;
 310   case eAdamsBashforth2: VState.vQtrn += dt*(1.5*vQtrndot - 0.5*last_vQtrndot);
 311     break;
 312   case eAdamsBashforth3: VState.vQtrn += (1/12.0)*dt*(23.0*vQtrndot - 16.0*last_vQtrndot + 5.0*last2_vQtrndot);
 313     break;
 314   case eNone: // do nothing, freeze angular position
 315     break;
 316   }
 317
 318   // Propagate translational position
 319
 320   switch(integrator_translational_position) {
 321   case eRectEuler:       VState.vLocation += dt*vLocationDot;
 322     break;
 323   case eTrapezoidal:     VState.vLocation += 0.5*dt*(vLocationDot + last_vLocationDot);
 324     break;
 325   case eAdamsBashforth2: VState.vLocation += dt*(1.5*vLocationDot - 0.5*last_vLocationDot);
 326     break;
 327   case eAdamsBashforth3: VState.vLocation += (1/12.0)*dt*(23.0*vLocationDot - 16.0*last_vLocationDot + 5.0*last2_vLocationDot);
 328     break;
 329   case eNone: // do nothing, freeze translational position
 330     break;
 331   }
 332
 333   // Set past values
 334
 335   last2_vPQRdot = last_vPQRdot;
 336   last_vPQRdot = vPQRdot;
 337
 338   last2_vUVWdot = last_vUVWdot;
 339   last_vUVWdot = vUVWdot;
 340
 341   last2_vQtrndot = last_vQtrndot;
 342   last_vQtrndot = vQtrndot;
 343
 344   last2_vLocationDot = last_vLocationDot;
 345   last_vLocationDot = vLocationDot;
 346
 347   Debug(2);
 348   return false;
 349 }
 350
 351 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 352 // Compute body frame rotational accelerations based on the current body moments
 353 //
 354 // vPQRdot is the derivative of the absolute angular velocity of the vehicle
 355 // (body rate with respect to the inertial frame), expressed in the body frame,
 356 // where the derivative is taken in the body frame.
 357 // J is the inertia matrix
 358 // Jinv is the inverse inertia matrix
 359 // vMoments is the moment vector in the body frame
 360 // vPQRi is the total inertial angular velocity of the vehicle
 361 // expressed in the body frame.
 362 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 363 //            Second edition (2004), eqn 1.5-16e (page 50)
 364
 365 void FGPropagate::CalculatePQRdot(void)
 366 {
 367   const FGColumnVector3& vMoments = Aircraft->GetMoments(); // current moments
 368   const FGMatrix33& J = MassBalance->GetJ();                // inertia matrix
 369   const FGMatrix33& Jinv = MassBalance->GetJinv();          // inertia matrix inverse
 370
 371   // Compute body frame rotational accelerations based on the current body
 372   // moments and the total inertial angular velocity expressed in the body
 373   // frame.
 374
 375   vPQRdot = Jinv*(vMoments - vPQRi*(J*vPQRi));
 376 }
 377
 378 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 379 // Compute the quaternion orientation derivative
 380 //
 381 // vQtrndot is the quaternion derivative.
 382 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 383 //            Second edition (2004), eqn 1.5-16b (page 50)
 384
 385 void FGPropagate::CalculateQuatdot(void)
 386 {
 387   vOmegaLocal.InitMatrix( radInv*vVel(eEast),
 388                          -radInv*vVel(eNorth),
 389                          -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
 390
 391   // Compute quaternion orientation derivative on current body rates
 392   vQtrndot = VState.vQtrn.GetQDot( VState.vPQR - Tl2b*vOmegaLocal);
 393 }
 394
 395 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 396 // This set of calculations results in the body frame accelerations being
 397 // computed.
 398 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 399 //            Second edition (2004), eqn 1.5-16d (page 50)
 400
 401 void FGPropagate::CalculateUVWdot(void)
 402 {
 403   double mass = MassBalance->GetMass();                      // mass
 404   const FGColumnVector3& vForces = Aircraft->GetForces();    // current forces
 405
 406   const FGColumnVector3 vGravAccel( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) );
 407
 408   // Begin to compute body frame accelerations based on the current body forces
 409   vUVWdot = vForces/mass - VState.vPQR * VState.vUVW;
 410
 411   // Include Coriolis acceleration.
 412   vUVWdot -= 2.0 * (Ti2b *vOmega) * VState.vUVW;
 413
 414   // Include Centrifugal acceleration.
 415   if (!GroundReactions->GetWOW()) {
 416     vUVWdot -= Ti2b*(vOmega*(vOmega*(Tec2i*VState.vLocation)));
 417   }
 418
 419   // Include Gravitation accel
 420   FGColumnVector3 gravAccel = Tl2b*vGravAccel;
 421   vUVWdot += gravAccel;
 422 }
 423
 424 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 425
 426 void FGPropagate::CalculateLocationdot(void)
 427 {
 428   // Transform the vehicle velocity relative to the ECEF frame, expressed
 429   // in the body frame, to be expressed in the ECEF frame.
 430   vLocationDot = Tb2ec * VState.vUVW;
 431
 432   // Now, transform the velocity vector of the body relative to the origin (Earth
 433   // center) to be expressed in the inertial frame, and add the vehicle velocity
 434   // contribution due to the rotation of the planet. The above velocity is only
 435   // relative to the rotating ECEF frame.
 436   // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 437   //            Second edition (2004), eqn 1.5-16c (page 50)
 438
 439   vInertialVelocity = Tec2i * vLocationDot + (vOmega * (Tec2i * VState.vLocation));
 440 }
 441
 442 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 443
 444 void FGPropagate::RecomputeLocalTerrainRadius(void)
 445 {
 446   double t = State->Getsim_time();
 447
 448   // Get the LocalTerrain radius.
 449   FGLocation contactloc;
 450   FGColumnVector3 dv;
 451   FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
 452   LocalTerrainRadius = contactloc.GetRadius();
 453 }
 454
 455 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 456
 457 void FGPropagate::SetTerrainElevation(double terrainElev)
 458 {
 459   LocalTerrainRadius = terrainElev + SeaLevelRadius;
 460   FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
 461 }
 462
 463 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 464
 465 double FGPropagate::GetTerrainElevation(void) const
 466 {
 467   return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
 468 }
 469
 470 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 471
 472 const FGMatrix33& FGPropagate::GetTi2ec(void)
 473 {
 474   return VState.vLocation.GetTi2ec(Inertial->GetEarthPositionAngle());
 475 }
 476
 477 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 478
 479 const FGMatrix33& FGPropagate::GetTec2i(void)
 480 {
 481   return VState.vLocation.GetTec2i(Inertial->GetEarthPositionAngle());
 482 }
 483
 484 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 485
 486 void FGPropagate::SetAltitudeASL(double altASL)
 487 {
 488   VState.vLocation.SetRadius( altASL + SeaLevelRadius );
 489 }
 490
 491 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 492
 493 double FGPropagate::GetLocalTerrainRadius(void) const
 494 {
 495   return LocalTerrainRadius;
 496 }
 497
 498 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 499
 500 double FGPropagate::GetDistanceAGL(void) const
 501 {
 502   return VState.vLocation.GetRadius() - LocalTerrainRadius;
 503 }
 504
 505 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 506
 507 void FGPropagate::SetDistanceAGL(double tt)
 508 {
 509   VState.vLocation.SetRadius( tt + LocalTerrainRadius );
 510 }
 511
 512 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 513
 514 void FGPropagate::bind(void)
 515 {
 516   typedef double (FGPropagate::*PMF)(int) const;
 517 //  typedef double (FGPropagate::*dPMF)() const;
 518   PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
 519
 520   PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
 521   PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
 522   PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
 523
 524   PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
 525   PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
 526   PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
 527
 528   PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
 529   PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
 530   PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
 531
 532   PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
 533   PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
 534   PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
 535
 536   PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
 537
 538   PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
 539   PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
 540   PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
 541
 542   PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
 543   PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
 544   PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
 545
 546   PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
 547   PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
 548   PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
 549   PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
 550   PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
 551   PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
 552   PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
 553   PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
 554   PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
 555   PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
 556   PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
 557   PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
 558                           &FGPropagate::GetTerrainElevation,
 559                           &FGPropagate::SetTerrainElevation, false);
 560
 561   PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
 562
 563   PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 564   PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 565   PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 566
 567   PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 568   PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 569   PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 570
 571   PropertyManager->Tie("simulation/integrator/rate/rotational", &integrator_rotational_rate);
 572   PropertyManager->Tie("simulation/integrator/rate/translational", &integrator_translational_rate);
 573   PropertyManager->Tie("simulation/integrator/position/rotational", &integrator_rotational_position);
 574   PropertyManager->Tie("simulation/integrator/position/translational", &integrator_translational_position);
 575 }
 576
 577 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 578 //    The bitmasked value choices are as follows:
 579 //    unset: In this case (the default) JSBSim would only print
 580 //       out the normally expected messages, essentially echoing
 581 //       the config files as they are read. If the environment
 582 //       variable is not set, debug_lvl is set to 1 internally
 583 //    0: This requests JSBSim not to output any messages
 584 //       whatsoever.
 585 //    1: This value explicity requests the normal JSBSim
 586 //       startup messages
 587 //    2: This value asks for a message to be printed out when
 588 //       a class is instantiated
 589 //    4: When this value is set, a message is displayed when a
 590 //       FGModel object executes its Run() method
 591 //    8: When this value is set, various runtime state variables
 592 //       are printed out periodically
 593 //    16: When set various parameters are sanity checked and
 594 //       a message is printed out when they go out of bounds
 595
 596 void FGPropagate::Debug(int from)
 597 {
 598   if (debug_lvl <= 0) return;
 599
 600   if (debug_lvl & 1) { // Standard console startup message output
 601     if (from == 0) { // Constructor
 602
 603     }
 604   }
 605   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 606     if (from == 0) cout << "Instantiated: FGPropagate" << endl;
 607     if (from == 1) cout << "Destroyed:    FGPropagate" << endl;
 608   }
 609   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 610   }
 611   if (debug_lvl & 8 ) { // Runtime state variables
 612   }
 613   if (debug_lvl & 16) { // Sanity checking
 614     if (from == 2) { // State sanity checking
 615       if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
 616         cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
 617         exit(-1);
 618       }
 619       if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
 620         cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
 621         exit(-1);
 622       }
 623       if (fabs(GetDistanceAGL()) > 1e10) {
 624         cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
 625         exit(-1);
 626       }
 627     }
 628   }
 629   if (debug_lvl & 64) {
 630     if (from == 0) { // Constructor
 631       cout << IdSrc << endl;
 632       cout << IdHdr << endl;
 633     }
 634   }
 635 }
 636 }