src/FDM/JSBSim/models/FGPropagate.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGPropagate.cpp
   4  Author:       Jon S. Berndt
   5  Date started: 01/05/99
   6  Purpose:      Integrate the EOM to determine instantaneous position
   7  Called by:    FGFDMExec
   8
   9  ------------- Copyright (C) 1999  Jon S. Berndt (jon@jsbsim.org) -------------
  10
  11  This program is free software; you can redistribute it and/or modify it under
  12  the terms of the GNU Lesser General Public License as published by the Free Software
  13  Foundation; either version 2 of the License, or (at your option) any later
  14  version.
  15
  16  This program is distributed in the hope that it will be useful, but WITHOUT
  17  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  18  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  19  details.
  20
  21  You should have received a copy of the GNU Lesser General Public License along with
  22  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  23  Place - Suite 330, Boston, MA  02111-1307, USA.
  24
  25  Further information about the GNU Lesser General Public License can also be found on
  26  the world wide web at http://www.gnu.org.
  27
  28 FUNCTIONAL DESCRIPTION
  29 --------------------------------------------------------------------------------
  30 This class encapsulates the integration of rates and accelerations to get the
  31 current position of the aircraft.
  32
  33 HISTORY
  34 --------------------------------------------------------------------------------
  35 01/05/99   JSB   Created
  36
  37 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  38 COMMENTS, REFERENCES,  and NOTES
  39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  40 [1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
  41     Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420  Naval Postgraduate
  42     School, January 1994
  43 [2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
  44     JSC 12960, July 1977
  45 [3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
  46     NASA-Ames", NASA CR-2497, January 1975
  47 [4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
  48     Wiley & Sons, 1979 ISBN 0-471-03032-5
  49 [5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
  50     1982 ISBN 0-471-08936-2
  51 [6] Erin Catto, "Iterative Dynamics with Temporal Coherence", February 22, 2005
  52
  53 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  54 INCLUDES
  55 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  56
  57 #include <cmath>
  58 #include <cstdlib>
  59 #include <iostream>
  60 #include <iomanip>
  61
  62 #include "FGPropagate.h"
  63 #include "FGGroundReactions.h"
  64 #include "FGFDMExec.h"
  65 #include "input_output/FGPropertyManager.h"
  66
  67 using namespace std;
  68
  69 namespace JSBSim {
  70
  71 static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.100 2011/11/06 18:14:51 bcoconni Exp $";
  72 static const char *IdHdr = ID_PROPAGATE;
  73
  74 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  75 CLASS IMPLEMENTATION
  76 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  77
  78 FGPropagate::FGPropagate(FGFDMExec* fdmex)
  79   : FGModel(fdmex),
  80     VehicleRadius(0)
  81 {
  82   Debug(0);
  83   Name = "FGPropagate";
  84
  85   vInertialVelocity.InitMatrix();
  86
  87   /// These define the indices use to select the various integrators.
  88   // eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
  89
  90   integrator_rotational_rate = eRectEuler;
  91   integrator_translational_rate = eAdamsBashforth2;
  92   integrator_rotational_position = eRectEuler;
  93   integrator_translational_position = eAdamsBashforth3;
  94
  95   VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
  96   VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
  97   VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
  98   VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0));
  99
 100   bind();
 101   Debug(0);
 102 }
 103
 104 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 105
 106 FGPropagate::~FGPropagate(void)
 107 {
 108   Debug(1);
 109 }
 110
 111 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 112
 113 bool FGPropagate::InitModel(void)
 114 {
 115   // For initialization ONLY:
 116   VState.vLocation.SetEllipse(in.SemiMajor, in.SemiMinor);
 117   VState.vLocation.SetAltitudeAGL(4.0, FDMExec->GetSimTime());
 118
 119   vInertialVelocity.InitMatrix();
 120
 121   VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 122   VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 123   VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
 124   VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
 125
 126   integrator_rotational_rate = eRectEuler;
 127   integrator_translational_rate = eAdamsBashforth2;
 128   integrator_rotational_position = eRectEuler;
 129   integrator_translational_position = eAdamsBashforth3;
 130
 131   return true;
 132 }
 133
 134 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 135
 136 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
 137 {
 138   // Initialize the State Vector elements and the transformation matrices
 139
 140   // Set the position lat/lon/radius
 141   VState.vLocation = FGIC->GetPosition();
 142
 143   Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
 144   Tec2i = Ti2ec.Transposed();          // ECEF to ECI frame transform
 145
 146   VState.vInertialPosition = Tec2i * VState.vLocation;
 147
 148   UpdateLocationMatrices();
 149
 150   // Set the orientation from the euler angles (is normalized within the
 151   // constructor). The Euler angles represent the orientation of the body
 152   // frame relative to the local frame.
 153   VState.qAttitudeLocal = FGIC->GetOrientation();
 154
 155   VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
 156   UpdateBodyMatrices();
 157
 158   // Set the velocities in the instantaneus body frame
 159   VState.vUVW = FGIC->GetUVWFpsIC();
 160
 161   // Compute the local frame ECEF velocity
 162   vVel = Tb2l * VState.vUVW;
 163
 164   // Compute local terrain velocity
 165   RecomputeLocalTerrainVelocity();
 166   VehicleRadius = GetRadius();
 167
 168   // Set the angular velocities of the body frame relative to the ECEF frame,
 169   // expressed in the body frame.
 170   VState.vPQR = FGIC->GetPQRRadpsIC();
 171
 172   VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
 173
 174   CalculateInertialVelocity(); // Translational position derivative
 175 }
 176
 177 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 178 // Initialize the past value deques
 179
 180 void FGPropagate::InitializeDerivatives()
 181 {
 182   for (int i=0; i<4; i++) {
 183     VState.dqPQRidot[i] = in.vPQRidot;
 184     VState.dqUVWidot[i] = in.vUVWidot;
 185     VState.dqInertialVelocity[i] = VState.vInertialVelocity;
 186     VState.dqQtrndot[i] = in.vQtrndot;
 187   }
 188 }
 189
 190 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 191 /*
 192 Purpose: Called on a schedule to perform EOM integration
 193 Notes:   [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
 194          In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
 195
 196 At the top of this Run() function, see several "shortcuts" (or, aliases) being
 197 set up for use later, rather than using the longer class->function() notation.
 198
 199 This propagation is done using the current state values
 200 and current derivatives. Based on these values we compute an approximation to the
 201 state values for (now + dt).
 202
 203 In the code below, variables named beginning with a small "v" refer to a
 204 a column vector, variables named beginning with a "T" refer to a transformation
 205 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
 206 Inertial.
 207
 208 */
 209
 210 bool FGPropagate::Run(bool Holding)
 211 {
 212   if (FGModel::Run(Holding)) return true;  // Fast return if we have nothing to do ...
 213   if (Holding) return false;
 214
 215   double dt = in.DeltaT * rate;  // The 'stepsize'
 216
 217   RunPreFunctions();
 218
 219   // Propagate rotational / translational velocity, angular /translational position, respectively.
 220
 221   Integrate(VState.vPQRi,             in.vPQRidot,          VState.dqPQRidot,          dt, integrator_rotational_rate);
 222   Integrate(VState.qAttitudeECI,      in.vQtrndot,          VState.dqQtrndot,          dt, integrator_rotational_position);
 223   Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
 224   Integrate(VState.vInertialVelocity, in.vUVWidot,          VState.dqUVWidot,          dt, integrator_translational_rate);
 225
 226   // CAUTION : the order of the operations below is very important to get transformation
 227   // matrices that are consistent with the new state of the vehicle
 228
 229   // 1. Update the Earth position angle (EPA)
 230   VState.vLocation.IncrementEarthPositionAngle(in.vOmegaPlanet(eZ)*(in.DeltaT*rate));
 231
 232   // 2. Update the Ti2ec and Tec2i transforms from the updated EPA
 233   Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
 234   Tec2i = Ti2ec.Transposed();          // ECEF to ECI frame transform
 235
 236   // 3. Update the location from the updated Ti2ec and inertial position
 237   VState.vLocation = Ti2ec*VState.vInertialPosition;
 238
 239   // 4. Update the other "Location-based" transformation matrices from the updated
 240   //    vLocation vector.
 241   UpdateLocationMatrices();
 242
 243   // 5. Normalize the ECI Attitude quaternion
 244   VState.qAttitudeECI.Normalize();
 245
 246   // 6. Update the "Orientation-based" transformation matrices from the updated
 247   //    orientation quaternion and vLocation vector.
 248   UpdateBodyMatrices();
 249
 250   // Translational position derivative (velocities are integrated in the inertial frame)
 251   CalculateUVW();
 252
 253   // Set auxilliary state variables
 254   RecomputeLocalTerrainVelocity();
 255   VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
 256
 257   VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
 258
 259   VState.qAttitudeLocal = Tl2b.GetQuaternion();
 260
 261   // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
 262   vVel = Tb2l * VState.vUVW;
 263
 264   RunPostFunctions();
 265
 266   Debug(2);
 267   return false;
 268 }
 269
 270 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 271   // Transform the velocity vector of the body relative to the origin (Earth
 272   // center) to be expressed in the inertial frame, and add the vehicle velocity
 273   // contribution due to the rotation of the planet.
 274   // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 275   //            Second edition (2004), eqn 1.5-16c (page 50)
 276
 277 void FGPropagate::CalculateInertialVelocity(void)
 278 {
 279   VState.vInertialVelocity = Tb2i * VState.vUVW + (in.vOmegaPlanet * VState.vInertialPosition);
 280 }
 281
 282 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 283   // Transform the velocity vector of the inertial frame to be expressed in the
 284   // body frame relative to the origin (Earth center), and substract the vehicle
 285   // velocity contribution due to the rotation of the planet.
 286
 287 void FGPropagate::CalculateUVW(void)
 288 {
 289   VState.vUVW = Ti2b * (VState.vInertialVelocity - (in.vOmegaPlanet * VState.vInertialPosition));
 290 }
 291
 292 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 293
 294 void FGPropagate::Integrate( FGColumnVector3& Integrand,
 295                              FGColumnVector3& Val,
 296                              deque <FGColumnVector3>& ValDot,
 297                              double dt,
 298                              eIntegrateType integration_type)
 299 {
 300   ValDot.push_front(Val);
 301   ValDot.pop_back();
 302
 303   switch(integration_type) {
 304   case eRectEuler:       Integrand += dt*ValDot[0];
 305     break;
 306   case eTrapezoidal:     Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
 307     break;
 308   case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
 309     break;
 310   case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
 311     break;
 312   case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
 313     break;
 314   case eNone: // do nothing, freeze translational rate
 315     break;
 316   }
 317 }
 318
 319 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 320
 321 void FGPropagate::Integrate( FGQuaternion& Integrand,
 322                              FGQuaternion& Val,
 323                              deque <FGQuaternion>& ValDot,
 324                              double dt,
 325                              eIntegrateType integration_type)
 326 {
 327   ValDot.push_front(Val);
 328   ValDot.pop_back();
 329
 330   switch(integration_type) {
 331   case eRectEuler:       Integrand += dt*ValDot[0];
 332     break;
 333   case eTrapezoidal:     Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
 334     break;
 335   case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
 336     break;
 337   case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
 338     break;
 339   case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
 340     break;
 341   case eNone: // do nothing, freeze rotational rate
 342     break;
 343   }
 344 }
 345
 346 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 347
 348 void FGPropagate::UpdateLocationMatrices(void)
 349 {
 350   Tl2ec = VState.vLocation.GetTl2ec(); // local to ECEF transform
 351   Tec2l = Tl2ec.Transposed();          // ECEF to local frame transform
 352   Ti2l  = VState.vLocation.GetTi2l();  // ECI to local frame transform
 353   Tl2i  = Ti2l.Transposed();           // local to ECI transform
 354 }
 355
 356 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 357
 358 void FGPropagate::UpdateBodyMatrices(void)
 359 {
 360   Ti2b  = VState.qAttitudeECI.GetT(); // ECI to body frame transform
 361   Tb2i  = Ti2b.Transposed();          // body to ECI frame transform
 362   Tl2b  = Ti2b * Tl2i;                // local to body frame transform
 363   Tb2l  = Tl2b.Transposed();          // body to local frame transform
 364   Tec2b = Ti2b * Tec2i;               // ECEF to body frame transform
 365   Tb2ec = Tec2b.Transposed();         // body to ECEF frame tranform
 366 }
 367
 368 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 369
 370 void FGPropagate::SetInertialOrientation(const FGQuaternion& Qi) {
 371   VState.qAttitudeECI = Qi;
 372   VState.qAttitudeECI.Normalize();
 373   UpdateBodyMatrices();
 374   VState.qAttitudeLocal = Tl2b.GetQuaternion();
 375 }
 376
 377 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 378
 379 void FGPropagate::SetInertialVelocity(const FGColumnVector3& Vi) {
 380   VState.vInertialVelocity = Vi;
 381   CalculateUVW();
 382   vVel = Tb2l * VState.vUVW;
 383 }
 384
 385 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 386
 387 void FGPropagate::SetInertialRates(const FGColumnVector3& vRates) {
 388   VState.vPQRi = Ti2b * vRates;
 389   VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
 390 }
 391
 392 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 393
 394 void FGPropagate::RecomputeLocalTerrainVelocity()
 395 {
 396   FGLocation contact;
 397   FGColumnVector3 normal;
 398   VState.vLocation.GetContactPoint(FDMExec->GetSimTime(), contact, normal,
 399                                    LocalTerrainVelocity, LocalTerrainAngularVelocity);
 400 }
 401
 402 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 403
 404 void FGPropagate::SetTerrainElevation(double terrainElev)
 405 {
 406   double radius = terrainElev + VState.vLocation.GetSeaLevelRadius();
 407   FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(radius);
 408 }
 409
 410
 411 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 412
 413 void FGPropagate::SetSeaLevelRadius(double tt)
 414 {
 415   FDMExec->GetGroundCallback()->SetSeaLevelRadius(tt);
 416 }
 417
 418 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 419
 420 double FGPropagate::GetLocalTerrainRadius(void) const
 421 {
 422   return VState.vLocation.GetTerrainRadius(FDMExec->GetSimTime());
 423 }
 424
 425 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 426
 427 double FGPropagate::GetDistanceAGL(void) const
 428 {
 429   return VState.vLocation.GetAltitudeAGL(FDMExec->GetSimTime());
 430 }
 431
 432 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 433
 434 void FGPropagate::SetDistanceAGL(double tt)
 435 {
 436   VState.vLocation.SetAltitudeAGL(tt, FDMExec->GetSimTime());
 437   UpdateVehicleState();
 438 }
 439
 440 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 441
 442 void FGPropagate::SetVState(const VehicleState& vstate)
 443 {
 444   //ToDo: Shouldn't all of these be set from the vstate vector passed in?
 445   VState.vLocation = vstate.vLocation;
 446   VState.vLocation.SetEarthPositionAngle(vstate.vLocation.GetEPA());
 447   Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
 448   Tec2i = Ti2ec.Transposed();
 449   UpdateLocationMatrices();
 450   SetInertialOrientation(vstate.qAttitudeECI);
 451   RecomputeLocalTerrainVelocity();
 452   VehicleRadius = GetRadius();
 453   VState.vUVW = vstate.vUVW;
 454   vVel = Tb2l * VState.vUVW;
 455   VState.vPQR = vstate.vPQR;
 456   VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
 457   VState.vInertialPosition = vstate.vInertialPosition;
 458 }
 459
 460 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 461
 462 void FGPropagate::UpdateVehicleState(void)
 463 {
 464   RecomputeLocalTerrainVelocity();
 465   VehicleRadius = GetRadius();
 466   VState.vInertialPosition = Tec2i * VState.vLocation;
 467   UpdateLocationMatrices();
 468   UpdateBodyMatrices();
 469   vVel = Tb2l * VState.vUVW;
 470   VState.qAttitudeLocal = Tl2b.GetQuaternion();
 471 }
 472
 473 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 474
 475 void FGPropagate::SetLocation(const FGLocation& l)
 476 {
 477   VState.vLocation = l;
 478   Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
 479   Tec2i = Ti2ec.Transposed();
 480   UpdateVehicleState();
 481 }
 482
 483 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 484
 485 void FGPropagate::DumpState(void)
 486 {
 487   cout << endl;
 488   cout << fgblue
 489        << "------------------------------------------------------------------" << reset << endl;
 490   cout << highint
 491        << "State Report at sim time: " << FDMExec->GetSimTime() << " seconds" << reset << endl;
 492   cout << "  " << underon
 493        <<   "Position" << underoff << endl;
 494   cout << "    ECI:   " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl;
 495   cout << "    ECEF:  " << VState.vLocation << " (x,y,z, in ft)"  << endl;
 496   cout << "    Local: " << VState.vLocation.GetLatitudeDeg()
 497                         << ", " << VState.vLocation.GetLongitudeDeg()
 498                         << ", " << GetAltitudeASL() << " (lat, lon, alt in deg and ft)" << endl;
 499
 500   cout << endl << "  " << underon
 501        <<   "Orientation" << underoff << endl;
 502   cout << "    ECI:   " << VState.qAttitudeECI.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
 503   cout << "    Local: " << VState.qAttitudeLocal.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
 504
 505   cout << endl << "  " << underon
 506        <<   "Velocity" << underoff << endl;
 507   cout << "    ECI:   " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl;
 508   cout << "    ECEF:  " << (Tb2ec * VState.vUVW).Dump(", ")  << " (x,y,z in ft/s)"  << endl;
 509   cout << "    Local: " << GetVel() << " (n,e,d in ft/sec)" << endl;
 510   cout << "    Body:  " << GetUVW() << " (u,v,w in ft/sec)" << endl;
 511
 512   cout << endl << "  " << underon
 513        <<   "Body Rates (relative to given frame, expressed in body frame)" << underoff << endl;
 514   cout << "    ECI:   " << (VState.vPQRi*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
 515   cout << "    ECEF:  " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
 516 }
 517
 518 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 519
 520 void FGPropagate::bind(void)
 521 {
 522   typedef double (FGPropagate::*PMF)(int) const;
 523
 524   PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
 525
 526   PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
 527   PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
 528   PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
 529
 530   PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
 531   PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
 532   PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
 533
 534   PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
 535   PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
 536   PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
 537
 538   PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
 539   PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
 540   PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
 541
 542   PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
 543
 544   PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
 545   PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
 546   PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude, false);
 547   PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude, false);
 548   PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg, false);
 549   PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg, false);
 550   PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
 551   PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
 552   PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
 553   PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
 554   PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
 555   PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
 556                           &FGPropagate::GetTerrainElevation,
 557                           &FGPropagate::SetTerrainElevation, false);
 558
 559   PropertyManager->Tie("position/epa-rad", this, &FGPropagate::GetEarthPositionAngle);
 560   PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
 561
 562   PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 563   PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 564   PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 565
 566   PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 567   PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 568   PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 569
 570   PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
 571   PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
 572   PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
 573   PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
 574 }
 575
 576 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 577 //    The bitmasked value choices are as follows:
 578 //    unset: In this case (the default) JSBSim would only print
 579 //       out the normally expected messages, essentially echoing
 580 //       the config files as they are read. If the environment
 581 //       variable is not set, debug_lvl is set to 1 internally
 582 //    0: This requests JSBSim not to output any messages
 583 //       whatsoever.
 584 //    1: This value explicity requests the normal JSBSim
 585 //       startup messages
 586 //    2: This value asks for a message to be printed out when
 587 //       a class is instantiated
 588 //    4: When this value is set, a message is displayed when a
 589 //       FGModel object executes its Run() method
 590 //    8: When this value is set, various runtime state variables
 591 //       are printed out periodically
 592 //    16: When set various parameters are sanity checked and
 593 //       a message is printed out when they go out of bounds
 594
 595 void FGPropagate::Debug(int from)
 596 {
 597   if (debug_lvl <= 0) return;
 598
 599   if (debug_lvl & 1) { // Standard console startup message output
 600     if (from == 0) { // Constructor
 601
 602     }
 603   }
 604   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 605     if (from == 0) cout << "Instantiated: FGPropagate" << endl;
 606     if (from == 1) cout << "Destroyed:    FGPropagate" << endl;
 607   }
 608   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 609   }
 610   if (debug_lvl & 8 && from == 2) { // Runtime state variables
 611     cout << endl << fgblue << highint << left
 612          << "  Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
 613          << reset << endl;
 614     cout << endl;
 615     cout << highint << "  Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
 616          << GetEarthPositionAngleDeg() << endl;
 617     cout << endl;
 618     cout << highint << "  Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
 619     cout << highint << "  Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
 620     cout << highint << "  Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
 621     cout << highint << "  Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
 622     cout << highint << "  Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
 623     cout << highint << "  Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
 624     cout << highint << "  Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
 625 //    cout << highint << "  Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
 626     cout << endl;
 627     cout << highint << "  Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
 628                     << reset << endl << Tec2b.Dump("\t", "    ") << endl;
 629     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 630                     << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
 631                     << endl << endl;
 632
 633     cout << highint << "  Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
 634                     << reset << endl << Tb2ec.Dump("\t", "    ") << endl;
 635     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 636                     << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
 637                     << endl << endl;
 638
 639     cout << highint << "  Matrix Local to Body (Orientation of Body with respect to Local):"
 640                     << reset << endl << Tl2b.Dump("\t", "    ") << endl;
 641     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 642                     << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
 643                     << endl << endl;
 644
 645     cout << highint << "  Matrix Body to Local (Orientation of Local with respect to Body):"
 646                     << reset << endl << Tb2l.Dump("\t", "    ") << endl;
 647     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 648                     << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
 649                     << endl << endl;
 650
 651     cout << highint << "  Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
 652                     << reset << endl << Tl2ec.Dump("\t", "    ") << endl;
 653     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 654                     << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
 655                     << endl << endl;
 656
 657     cout << highint << "  Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
 658                     << reset << endl << Tec2l.Dump("\t", "    ") << endl;
 659     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 660                     << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
 661                     << endl << endl;
 662
 663     cout << highint << "  Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
 664                     << reset << endl << Tec2i.Dump("\t", "    ") << endl;
 665     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 666                     << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
 667                     << endl << endl;
 668
 669     cout << highint << "  Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
 670                     << reset << endl << Ti2ec.Dump("\t", "    ") << endl;
 671     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 672                     << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
 673                     << endl << endl;
 674
 675     cout << highint << "  Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
 676                     << reset << endl << Ti2b.Dump("\t", "    ") << endl;
 677     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 678                     << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
 679                     << endl << endl;
 680
 681     cout << highint << "  Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
 682                     << reset << endl << Tb2i.Dump("\t", "    ") << endl;
 683     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 684                     << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
 685                     << endl << endl;
 686
 687     cout << highint << "  Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
 688                     << reset << endl << Ti2l.Dump("\t", "    ") << endl;
 689     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 690                     << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
 691                     << endl << endl;
 692
 693     cout << highint << "  Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
 694                     << reset << endl << Tl2i.Dump("\t", "    ") << endl;
 695     cout << highint << "    Associated Euler angles (deg): " << setw(8)
 696                     << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
 697                     << endl << endl;
 698
 699     cout << setprecision(6); // reset the output stream
 700   }
 701   if (debug_lvl & 16) { // Sanity checking
 702     if (from == 2) { // State sanity checking
 703       if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
 704         cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
 705         exit(-1);
 706       }
 707       if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
 708         cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
 709         exit(-1);
 710       }
 711       if (fabs(GetDistanceAGL()) > 1e10) {
 712         cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
 713         exit(-1);
 714       }
 715     }
 716   }
 717   if (debug_lvl & 64) {
 718     if (from == 0) { // Constructor
 719       cout << IdSrc << endl;
 720       cout << IdHdr << endl;
 721     }
 722   }
 723 }
 724 }