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