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