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