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] S. Buss, "Accurate and Efficient Simulation of Rigid Body Rotations",
  52     Technical Report, Department of Mathematics, University of California,
  53     San Diego, 1999
  54 [7] Barker L.E., Bowles R.L. and Williams L.H., "Development and Application of
  55     a Local Linearization Algorithm for the Integration of Quaternion Rate
  56     Equations in Real-Time Flight Simulation Problems", NASA TN D-7347,
  57     December 1973
  58 [8] Phillips W.F, Hailey C.E and Gebert G.A, "Review of Attitude Representations
  59     Used for Aircraft Kinematics", Journal Of Aircraft Vol. 38, No. 4,
  60     July-August 2001
  61
  62 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  63 INCLUDES
  64 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  65
  66 #include <cmath>
  67 #include <cstdlib>
  68 #include <iostream>
  69 #include <iomanip>
  70
  71 #include "FGPropagate.h"
  72 #include "FGGroundReactions.h"
  73 #include "FGFDMExec.h"
  74 #include "input_output/FGPropertyManager.h"
  75
  76 using namespace std;
  77
  78 namespace JSBSim {
  79
  80 static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.105 2012/03/26 21:26:11 bcoconni Exp $";
  81 static const char *IdHdr = ID_PROPAGATE;
  82
  83 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  84 CLASS IMPLEMENTATION
  85 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  86
  87 FGPropagate::FGPropagate(FGFDMExec* fdmex)
  88   : FGModel(fdmex),
  89     VehicleRadius(0)
  90 {
  91   Debug(0);
  92   Name = "FGPropagate";
  93
  94   vInertialVelocity.InitMatrix();
  95
  96   /// These define the indices use to select the various integrators.
  97   // eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
  98
  99   integrator_rotational_rate = eRectEuler;
 100   integrator_translational_rate = eAdamsBashforth2;
 101   integrator_rotational_position = eRectEuler;
 102   integrator_translational_position = eAdamsBashforth3;
 103
 104   VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 105   VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 106   VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
 107   VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0));
 108
 109   bind();
 110   Debug(0);
 111 }
 112
 113 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 114
 115 FGPropagate::~FGPropagate(void)
 116 {
 117   Debug(1);
 118 }
 119
 120 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 121
 122 bool FGPropagate::InitModel(void)
 123 {
 124   // For initialization ONLY:
 125   VState.vLocation.SetEllipse(in.SemiMajor, in.SemiMinor);
 126   VState.vLocation.SetAltitudeAGL(4.0, FDMExec->GetSimTime());
 127
 128   vInertialVelocity.InitMatrix();
 129
 130   VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 131   VState.dqUVWidot.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 = eRectEuler;
 136   integrator_translational_rate = eAdamsBashforth2;
 137   integrator_rotational_position = eRectEuler;
 138   integrator_translational_position = eAdamsBashforth3;
 139
 140   return true;
 141 }
 142
 143 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 144
 145 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
 146 {
 147   // Initialize the State Vector elements and the transformation matrices
 148
 149   // Set the position lat/lon/radius
 150   VState.vLocation = FGIC->GetPosition();
 151
 152   Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
 153   Tec2i = Ti2ec.Transposed();          // ECEF to ECI frame transform
 154
 155   VState.vInertialPosition = Tec2i * VState.vLocation;
 156
 157   UpdateLocationMatrices();
 158
 159   // Set the orientation from the euler angles (is normalized within the
 160   // constructor). The Euler angles represent the orientation of the body
 161   // frame relative to the local frame.
 162   VState.qAttitudeLocal = FGIC->GetOrientation();
 163
 164   VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
 165   UpdateBodyMatrices();
 166
 167   // Set the velocities in the instantaneus body frame
 168   VState.vUVW = FGIC->GetUVWFpsIC();
 169
 170   // Compute the local frame ECEF velocity
 171   vVel = Tb2l * VState.vUVW;
 172
 173   // Compute local terrain velocity
 174   RecomputeLocalTerrainVelocity();
 175   VehicleRadius = GetRadius();
 176
 177   // Set the angular velocities of the body frame relative to the ECEF frame,
 178   // expressed in the body frame.
 179   VState.vPQR = FGIC->GetPQRRadpsIC();
 180
 181   VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
 182
 183   CalculateInertialVelocity(); // Translational position derivative
 184 }
 185
 186 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 187 // Initialize the past value deques
 188
 189 void FGPropagate::InitializeDerivatives()
 190 {
 191   for (int i=0; i<4; i++) {
 192     VState.dqPQRidot[i] = in.vPQRidot;
 193     VState.dqUVWidot[i] = in.vUVWidot;
 194     VState.dqInertialVelocity[i] = VState.vInertialVelocity;
 195     VState.dqQtrndot[i] = in.vQtrndot;
 196   }
 197 }
 198
 199 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 200 /*
 201 Purpose: Called on a schedule to perform EOM integration
 202 Notes:   [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
 203          In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
 204
 205 At the top of this Run() function, see several "shortcuts" (or, aliases) being
 206 set up for use later, rather than using the longer class->function() notation.
 207
 208 This propagation is done using the current state values
 209 and current derivatives. Based on these values we compute an approximation to the
 210 state values for (now + dt).
 211
 212 In the code below, variables named beginning with a small "v" refer to a
 213 a column vector, variables named beginning with a "T" refer to a transformation
 214 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
 215 Inertial.
 216
 217 */
 218
 219 bool FGPropagate::Run(bool Holding)
 220 {
 221   if (FGModel::Run(Holding)) return true;  // Fast return if we have nothing to do ...
 222   if (Holding) return false;
 223
 224   double dt = in.DeltaT * rate;  // The 'stepsize'
 225
 226   // Propagate rotational / translational velocity, angular /translational position, respectively.
 227
 228   Integrate(VState.qAttitudeECI,      in.vQtrndot,          VState.dqQtrndot,          dt, integrator_rotational_position);
 229   Integrate(VState.vPQRi,             in.vPQRidot,          VState.dqPQRidot,          dt, integrator_rotational_rate);
 230   Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
 231   Integrate(VState.vInertialVelocity, in.vUVWidot,          VState.dqUVWidot,          dt, integrator_translational_rate);
 232
 233   // CAUTION : the order of the operations below is very important to get transformation
 234   // matrices that are consistent with the new state of the vehicle
 235
 236   // 1. Update the Earth position angle (EPA)
 237   VState.vLocation.IncrementEarthPositionAngle(in.vOmegaPlanet(eZ)*(in.DeltaT*rate));
 238
 239   // 2. Update the Ti2ec and Tec2i transforms from the updated EPA
 240   Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
 241   Tec2i = Ti2ec.Transposed();          // ECEF to ECI frame transform
 242
 243   // 3. Update the location from the updated Ti2ec and inertial position
 244   VState.vLocation = Ti2ec*VState.vInertialPosition;
 245
 246   // 4. Update the other "Location-based" transformation matrices from the updated
 247   //    vLocation vector.
 248   UpdateLocationMatrices();
 249
 250   // 5. Update the "Orientation-based" transformation matrices from the updated
 251   //    orientation quaternion and vLocation vector.
 252   UpdateBodyMatrices();
 253
 254   // Translational position derivative (velocities are integrated in the inertial frame)
 255   CalculateUVW();
 256
 257   // Set auxilliary state variables
 258   RecomputeLocalTerrainVelocity();
 259   VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
 260
 261   VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
 262
 263   VState.qAttitudeLocal = Tl2b.GetQuaternion();
 264
 265   // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
 266   vVel = Tb2l * VState.vUVW;
 267
 268   Debug(2);
 269   return false;
 270 }
 271
 272 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 273   // Transform the velocity vector of the body relative to the origin (Earth
 274   // center) to be expressed in the inertial frame, and add the vehicle velocity
 275   // contribution due to the rotation of the planet.
 276   // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 277   //            Second edition (2004), eqn 1.5-16c (page 50)
 278
 279 void FGPropagate::CalculateInertialVelocity(void)
 280 {
 281   VState.vInertialVelocity = Tb2i * VState.vUVW + (in.vOmegaPlanet * VState.vInertialPosition);
 282 }
 283
 284 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 285   // Transform the velocity vector of the inertial frame to be expressed in the
 286   // body frame relative to the origin (Earth center), and substract the vehicle
 287   // velocity contribution due to the rotation of the planet.
 288
 289 void FGPropagate::CalculateUVW(void)
 290 {
 291   VState.vUVW = Ti2b * (VState.vInertialVelocity - (in.vOmegaPlanet * VState.vInertialPosition));
 292 }
 293
 294 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 295
 296 void FGPropagate::Integrate( FGColumnVector3& Integrand,
 297                              FGColumnVector3& Val,
 298                              deque <FGColumnVector3>& ValDot,
 299                              double dt,
 300                              eIntegrateType integration_type)
 301 {
 302   ValDot.push_front(Val);
 303   ValDot.pop_back();
 304
 305   switch(integration_type) {
 306   case eRectEuler:       Integrand += dt*ValDot[0];
 307     break;
 308   case eTrapezoidal:     Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
 309     break;
 310   case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
 311     break;
 312   case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
 313     break;
 314   case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
 315     break;
 316   case eNone: // do nothing, freeze translational rate
 317     break;
 318   default:
 319     break;
 320   }
 321 }
 322
 323 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 324
 325 void FGPropagate::Integrate( FGQuaternion& Integrand,
 326                              FGQuaternion& Val,
 327                              deque <FGQuaternion>& ValDot,
 328                              double dt,
 329                              eIntegrateType integration_type)
 330 {
 331   ValDot.push_front(Val);
 332   ValDot.pop_back();
 333
 334   switch(integration_type) {
 335   case eRectEuler:       Integrand += dt*ValDot[0];
 336     break;
 337   case eTrapezoidal:     Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
 338     break;
 339   case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
 340     break;
 341   case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
 342     break;
 343   case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
 344     break;
 345   case eBuss1:
 346     {
 347       // This is the first order method as described in Samuel R. Buss paper[6].
 348       // The formula from Buss' paper is transposed below to quaternions and is
 349       // actually the exact solution of the quaternion differential equation
 350       // qdot = 1/2*w*q when w is constant.
 351       Integrand = Integrand * QExp(0.5 * dt * VState.vPQRi);
 352     }
 353     return; // No need to normalize since the quaternion exponential is always normal
 354   case eBuss2:
 355     {
 356       // This is the 'augmented second-order method' from S.R. Buss paper [6].
 357       // Unlike Runge-Kutta or Adams-Bashforth, it is a one-pass second-order
 358       // method (see reference [6]).
 359       FGColumnVector3 wi = VState.vPQRi;
 360       FGColumnVector3 wdoti = in.vPQRidot;
 361       FGColumnVector3 omega = wi + 0.5*dt*wdoti + dt*dt/12.*wdoti*wi;
 362       Integrand = Integrand * QExp(0.5 * dt * omega);
 363     }
 364     return; // No need to normalize since the quaternion exponential is always normal
 365   case eLocalLinearization:
 366     {
 367       // This is the local linearization algorithm of Barker et al. (see ref. [7])
 368       // It is also a one-pass second-order method. The code below is based on the
 369       // more compact formulation issued from equation (107) of ref. [8]. The
 370       // constants C1, C2, C3 and C4 have the same value than those in ref. [7] pp. 11
 371       FGColumnVector3 wi = 0.5 * VState.vPQRi;
 372       FGColumnVector3 wdoti = 0.5 * in.vPQRidot;
 373       double omegak2 = DotProduct(VState.vPQRi, VState.vPQRi);
 374       double omegak = omegak2 > 1E-6 ? sqrt(omegak2) : 1E-6;
 375       double rhok = 0.5 * dt * omegak;
 376       double C1 = cos(rhok);
 377       double C2 = 2.0 * sin(rhok) / omegak;
 378       double C3 = 4.0 * (1.0 - C1) / (omegak*omegak);
 379       double C4 = 4.0 * (dt - C2) / (omegak*omegak);
 380       FGColumnVector3 Omega = C2*wi + C3*wdoti + C4*wi*wdoti;
 381       FGQuaternion q;
 382
 383       q(1) = C1 - C4*DotProduct(wi, wdoti);
 384       q(2) = Omega(eP);
 385       q(3) = Omega(eQ);
 386       q(4) = Omega(eR);
 387
 388       Integrand = Integrand * q;
 389
 390       /* Cross check with ref. [7] pp.11-12 formulas and code pp. 20
 391       double pk = VState.vPQRi(eP);
 392       double qk = VState.vPQRi(eQ);
 393       double rk = VState.vPQRi(eR);
 394       double pdotk = in.vPQRidot(eP);
 395       double qdotk = in.vPQRidot(eQ);
 396       double rdotk = in.vPQRidot(eR);
 397       double Ap = -0.25 * (pk*pdotk + qk*qdotk + rk*rdotk);
 398       double Bp = 0.25 * (pk*qdotk - qk*pdotk);
 399       double Cp = 0.25 * (pdotk*rk - pk*rdotk);
 400       double Dp = 0.25 * (qk*rdotk - qdotk*rk);
 401       double C2p = sin(rhok) / omegak;
 402       double C3p = 2.0 * (1.0 - cos(rhok)) / (omegak*omegak);
 403       double H = C1 + C4 * Ap;
 404       double G = -C2p*rk - C3p*rdotk + C4*Bp;
 405       double J = C2p*qk + C3p*qdotk - C4*Cp;
 406       double K = C2p*pk + C3p*pdotk - C4*Dp;
 407
 408       cout << "q:       " << q << endl;
 409
 410       // Warning! In the paper of Barker et al. the quaternion components are not
 411       // ordered the same way as in JSBSim (see equations (2) and (3) of ref. [7]
 412       // as well as the comment just below equation (3))
 413       cout << "FORTRAN: " << H << " , " << K << " , " << J << " , " << -G << endl;*/
 414     }
 415     break; // The quaternion q is not normal so the normalization needs to be done.
 416   case eNone: // do nothing, freeze rotational rate
 417     break;
 418   default:
 419     break;
 420   }
 421
 422   Integrand.Normalize();
 423 }
 424
 425 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 426
 427 void FGPropagate::UpdateLocationMatrices(void)
 428 {
 429   Tl2ec = VState.vLocation.GetTl2ec(); // local to ECEF transform
 430   Tec2l = Tl2ec.Transposed();          // ECEF to local frame transform
 431   Ti2l  = VState.vLocation.GetTi2l();  // ECI to local frame transform
 432   Tl2i  = Ti2l.Transposed();           // local to ECI transform
 433 }
 434
 435 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 436
 437 void FGPropagate::UpdateBodyMatrices(void)
 438 {
 439   Ti2b  = VState.qAttitudeECI.GetT(); // ECI to body frame transform
 440   Tb2i  = Ti2b.Transposed();          // body to ECI frame transform
 441   Tl2b  = Ti2b * Tl2i;                // local to body frame transform
 442   Tb2l  = Tl2b.Transposed();          // body to local frame transform
 443   Tec2b = Ti2b * Tec2i;               // ECEF to body frame transform
 444   Tb2ec = Tec2b.Transposed();         // body to ECEF frame tranform
 445 }
 446
 447 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 448
 449 void FGPropagate::SetInertialOrientation(const FGQuaternion& Qi) {
 450   VState.qAttitudeECI = Qi;
 451   VState.qAttitudeECI.Normalize();
 452   UpdateBodyMatrices();
 453   VState.qAttitudeLocal = Tl2b.GetQuaternion();
 454 }
 455
 456 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 457
 458 void FGPropagate::SetInertialVelocity(const FGColumnVector3& Vi) {
 459   VState.vInertialVelocity = Vi;
 460   CalculateUVW();
 461   vVel = Tb2l * VState.vUVW;
 462 }
 463
 464 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 465
 466 void FGPropagate::SetInertialRates(const FGColumnVector3& vRates) {
 467   VState.vPQRi = Ti2b * vRates;
 468   VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
 469 }
 470
 471 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 472
 473 void FGPropagate::RecomputeLocalTerrainVelocity()
 474 {
 475   FGLocation contact;
 476   FGColumnVector3 normal;
 477   VState.vLocation.GetContactPoint(FDMExec->GetSimTime(), contact, normal,
 478                                    LocalTerrainVelocity, LocalTerrainAngularVelocity);
 479 }
 480
 481 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 482
 483 void FGPropagate::SetTerrainElevation(double terrainElev)
 484 {
 485   double radius = terrainElev + VState.vLocation.GetSeaLevelRadius();
 486   FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(radius);
 487 }
 488
 489
 490 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 491
 492 void FGPropagate::SetSeaLevelRadius(double tt)
 493 {
 494   FDMExec->GetGroundCallback()->SetSeaLevelRadius(tt);
 495 }
 496
 497 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 498
 499 double FGPropagate::GetLocalTerrainRadius(void) const
 500 {
 501   return VState.vLocation.GetTerrainRadius(FDMExec->GetSimTime());
 502 }
 503
 504 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 505
 506 double FGPropagate::GetDistanceAGL(void) const
 507 {
 508   return VState.vLocation.GetAltitudeAGL(FDMExec->GetSimTime());
 509 }
 510
 511 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 512
 513 void FGPropagate::SetDistanceAGL(double tt)
 514 {
 515   VState.vLocation.SetAltitudeAGL(tt, FDMExec->GetSimTime());
 516   UpdateVehicleState();
 517 }
 518
 519 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 520
 521 void FGPropagate::SetVState(const VehicleState& vstate)
 522 {
 523   //ToDo: Shouldn't all of these be set from the vstate vector passed in?
 524   VState.vLocation = vstate.vLocation;
 525   Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
 526   Tec2i = Ti2ec.Transposed();
 527   UpdateLocationMatrices();
 528   SetInertialOrientation(vstate.qAttitudeECI);
 529   RecomputeLocalTerrainVelocity();
 530   VehicleRadius = GetRadius();
 531   VState.vUVW = vstate.vUVW;
 532   vVel = Tb2l * VState.vUVW;
 533   VState.vPQR = vstate.vPQR;
 534   VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
 535   VState.vInertialPosition = vstate.vInertialPosition;
 536 }
 537
 538 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 539
 540 void FGPropagate::UpdateVehicleState(void)
 541 {
 542   RecomputeLocalTerrainVelocity();
 543   VehicleRadius = GetRadius();
 544   VState.vInertialPosition = Tec2i * VState.vLocation;
 545   UpdateLocationMatrices();
 546   UpdateBodyMatrices();
 547   vVel = Tb2l * VState.vUVW;
 548   VState.qAttitudeLocal = Tl2b.GetQuaternion();
 549 }
 550
 551 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 552
 553 void FGPropagate::SetLocation(const FGLocation& l)
 554 {
 555   VState.vLocation = l;
 556   Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
 557   Tec2i = Ti2ec.Transposed();
 558   UpdateVehicleState();
 559 }
 560
 561 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 562
 563 void FGPropagate::DumpState(void)
 564 {
 565   cout << endl;
 566   cout << fgblue
 567        << "------------------------------------------------------------------" << reset << endl;
 568   cout << highint
 569        << "State Report at sim time: " << FDMExec->GetSimTime() << " seconds" << reset << endl;
 570   cout << "  " << underon
 571        <<   "Position" << underoff << endl;
 572   cout << "    ECI:   " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl;
 573   cout << "    ECEF:  " << VState.vLocation << " (x,y,z, in ft)"  << endl;
 574   cout << "    Local: " << VState.vLocation.GetLatitudeDeg()
 575                         << ", " << VState.vLocation.GetLongitudeDeg()
 576                         << ", " << GetAltitudeASL() << " (lat, lon, alt in deg and ft)" << endl;
 577
 578   cout << endl << "  " << underon
 579        <<   "Orientation" << underoff << endl;
 580   cout << "    ECI:   " << VState.qAttitudeECI.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
 581   cout << "    Local: " << VState.qAttitudeLocal.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
 582
 583   cout << endl << "  " << underon
 584        <<   "Velocity" << underoff << endl;
 585   cout << "    ECI:   " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl;
 586   cout << "    ECEF:  " << (Tb2ec * VState.vUVW).Dump(", ")  << " (x,y,z in ft/s)"  << endl;
 587   cout << "    Local: " << GetVel() << " (n,e,d in ft/sec)" << endl;
 588   cout << "    Body:  " << GetUVW() << " (u,v,w in ft/sec)" << endl;
 589
 590   cout << endl << "  " << underon
 591        <<   "Body Rates (relative to given frame, expressed in body frame)" << underoff << endl;
 592   cout << "    ECI:   " << (VState.vPQRi*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
 593   cout << "    ECEF:  " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
 594 }
 595
 596 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 597
 598 void FGPropagate::bind(void)
 599 {
 600   typedef double (FGPropagate::*PMF)(int) const;
 601
 602   PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
 603
 604   PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
 605   PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
 606   PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
 607
 608   PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
 609   PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
 610   PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
 611
 612   PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
 613   PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
 614   PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
 615
 616   PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
 617   PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
 618   PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
 619
 620   PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
 621
 622   PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
 623   PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
 624   PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude, false);
 625   PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude, false);
 626   PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg, false);
 627   PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg, false);
 628   PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
 629   PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
 630   PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
 631   PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
 632   PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
 633   PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
 634                           &FGPropagate::GetTerrainElevation,
 635                           &FGPropagate::SetTerrainElevation, false);
 636
 637   PropertyManager->Tie("position/epa-rad", this, &FGPropagate::GetEarthPositionAngle);
 638   PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
 639
 640   PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 641   PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 642   PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 643
 644   PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 645   PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 646   PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 647
 648   PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
 649   PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
 650   PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
 651   PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
 652 }
 653
 654 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 655 //    The bitmasked value choices are as follows:
 656 //    unset: In this case (the default) JSBSim would only print
 657 //       out the normally expected messages, essentially echoing
 658 //       the config files as they are read. If the environment
 659 //       variable is not set, debug_lvl is set to 1 internally
 660 //    0: This requests JSBSim not to output any messages
 661 //       whatsoever.
 662 //    1: This value explicity requests the normal JSBSim
 663 //       startup messages
 664 //    2: This value asks for a message to be printed out when
 665 //       a class is instantiated
 666 //    4: When this value is set, a message is displayed when a
 667 //       FGModel object executes its Run() method
 668 //    8: When this value is set, various runtime state variables
 669 //       are printed out periodically
 670 //    16: When set various parameters are sanity checked and
 671 //       a message is printed out when they go out of bounds
 672
 673 void FGPropagate::Debug(int from)
 674 {
 675   if (debug_lvl <= 0) return;
 676
 677   if (debug_lvl & 1) { // Standard console startup message output
 678     if (from == 0) { // Constructor
 679
 680     }
 681   }
 682   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 683     if (from == 0) cout << "Instantiated: FGPropagate" << endl;
 684     if (from == 1) cout << "Destroyed:    FGPropagate" << endl;
 685   }
 686   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 687   }
 688   if (debug_lvl & 8 && from == 2) { // Runtime state variables
 689     cout << endl << fgblue << highint << left
 690          << "  Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
 691          << reset << endl;
 692     cout << endl;
 693     cout << highint << "  Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
 694          << GetEarthPositionAngleDeg() << endl;
 695     cout << endl;
 696     cout << highint << "  Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
 697     cout << highint << "  Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
 698     cout << highint << "  Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
 699     cout << highint << "  Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
 700     cout << highint << "  Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
 701     cout << highint << "  Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
 702     cout << highint << "  Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
 703 //    cout << highint << "  Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
 704     cout << endl;
 705     cout << highint << "  Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
 706                     << reset << endl << Tec2b.Dump("\t", "    ") << endl;
 707     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 708                     << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
 709                     << endl << endl;
 710
 711     cout << highint << "  Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
 712                     << reset << endl << Tb2ec.Dump("\t", "    ") << endl;
 713     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 714                     << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
 715                     << endl << endl;
 716
 717     cout << highint << "  Matrix Local to Body (Orientation of Body with respect to Local):"
 718                     << reset << endl << Tl2b.Dump("\t", "    ") << endl;
 719     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 720                     << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
 721                     << endl << endl;
 722
 723     cout << highint << "  Matrix Body to Local (Orientation of Local with respect to Body):"
 724                     << reset << endl << Tb2l.Dump("\t", "    ") << endl;
 725     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 726                     << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
 727                     << endl << endl;
 728
 729     cout << highint << "  Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
 730                     << reset << endl << Tl2ec.Dump("\t", "    ") << endl;
 731     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 732                     << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
 733                     << endl << endl;
 734
 735     cout << highint << "  Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
 736                     << reset << endl << Tec2l.Dump("\t", "    ") << endl;
 737     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 738                     << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
 739                     << endl << endl;
 740
 741     cout << highint << "  Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
 742                     << reset << endl << Tec2i.Dump("\t", "    ") << endl;
 743     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 744                     << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
 745                     << endl << endl;
 746
 747     cout << highint << "  Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
 748                     << reset << endl << Ti2ec.Dump("\t", "    ") << endl;
 749     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 750                     << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
 751                     << endl << endl;
 752
 753     cout << highint << "  Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
 754                     << reset << endl << Ti2b.Dump("\t", "    ") << endl;
 755     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 756                     << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
 757                     << endl << endl;
 758
 759     cout << highint << "  Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
 760                     << reset << endl << Tb2i.Dump("\t", "    ") << endl;
 761     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 762                     << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
 763                     << endl << endl;
 764
 765     cout << highint << "  Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
 766                     << reset << endl << Ti2l.Dump("\t", "    ") << endl;
 767     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 768                     << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
 769                     << endl << endl;
 770
 771     cout << highint << "  Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
 772                     << reset << endl << Tl2i.Dump("\t", "    ") << endl;
 773     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 774                     << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
 775                     << endl << endl;
 776
 777     cout << setprecision(6); // reset the output stream
 778   }
 779   if (debug_lvl & 16) { // Sanity checking
 780     if (from == 2) { // State sanity checking
 781       if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
 782         cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
 783         exit(-1);
 784       }
 785       if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
 786         cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
 787         exit(-1);
 788       }
 789       if (fabs(GetDistanceAGL()) > 1e10) {
 790         cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
 791         exit(-1);
 792       }
 793     }
 794   }
 795   if (debug_lvl & 64) {
 796     if (from == 0) { // Constructor
 797       cout << IdSrc << endl;
 798       cout << IdHdr << endl;
 799     }
 800   }
 801 }
 802 }