src/FDM/JSBSim/models/FGPropagate.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGPropagate.cpp
   4  Author:       Jon S. Berndt
   5  Date started: 01/05/99
   6  Purpose:      Integrate the EOM to determine instantaneous position
   7  Called by:    FGFDMExec
   8
   9  ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
  10
  11  This program is free software; you can redistribute it and/or modify it under
  12  the terms of the GNU 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 General Public License for more
  19  details.
  20
  21  You should have received a copy of the GNU 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 General Public License can also be found on
  26  the world wide web at http://www.gnu.org.
  27
  28 FUNCTIONAL DESCRIPTION
  29 --------------------------------------------------------------------------------
  30 This class encapsulates the integration of rates and accelerations to get the
  31 current position of the aircraft.
  32
  33 HISTORY
  34 --------------------------------------------------------------------------------
  35 01/05/99   JSB   Created
  36
  37 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  38 COMMENTS, REFERENCES,  and NOTES
  39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  40 [1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
  41     Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420  Naval Postgraduate
  42     School, January 1994
  43 [2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
  44     JSC 12960, July 1977
  45 [3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
  46     NASA-Ames", NASA CR-2497, January 1975
  47 [4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
  48     Wiley & Sons, 1979 ISBN 0-471-03032-5
  49 [5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
  50     1982 ISBN 0-471-08936-2
  51
  52 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  53 INCLUDES
  54 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  55
  56 #ifdef FGFS
  57 #  include <simgear/compiler.h>
  58 #  ifdef SG_HAVE_STD_INCLUDES
  59 #    include <cmath>
  60 #    include <iomanip>
  61 #  else
  62 #    include <math.h>
  63 #    include <iomanip.h>
  64 #  endif
  65 #else
  66 #  if defined(sgi) && !defined(__GNUC__)
  67 #    include <math.h>
  68 #    if (_COMPILER_VERSION < 740)
  69 #      include <iomanip.h>
  70 #    else
  71 #      include <iomanip>
  72 #    endif
  73 #  else
  74 #    include <cmath>
  75 #    include <iomanip>
  76 #  endif
  77 #endif
  78
  79 #include "FGPropagate.h"
  80 #include <FGState.h>
  81 #include <FGFDMExec.h>
  82 #include "FGAircraft.h"
  83 #include "FGMassBalance.h"
  84 #include "FGInertial.h"
  85 #include <input_output/FGPropertyManager.h>
  86
  87 namespace JSBSim {
  88
  89 static const char *IdSrc = "$Id$";
  90 static const char *IdHdr = ID_PROPAGATE;
  91
  92 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  93 CLASS IMPLEMENTATION
  94 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  95
  96 FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
  97 {
  98   Name = "FGPropagate";
  99
 100   bind();
 101   Debug(0);
 102 }
 103
 104 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 105
 106 FGPropagate::~FGPropagate(void)
 107 {
 108   unbind();
 109   Debug(1);
 110 }
 111
 112 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 113
 114 bool FGPropagate::InitModel(void)
 115 {
 116   FGModel::InitModel();
 117
 118   SeaLevelRadius = Inertial->RefRadius();          // For initialization ONLY
 119   RunwayRadius   = SeaLevelRadius;
 120
 121   VState.vLocation.SetRadius( SeaLevelRadius + 4.0 );
 122
 123   return true;
 124 }
 125
 126 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 127
 128 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
 129 {
 130   SeaLevelRadius = FGIC->GetSeaLevelRadiusFtIC();
 131   RunwayRadius = SeaLevelRadius;
 132
 133   // Set the position lat/lon/radius
 134   VState.vLocation = FGLocation( FGIC->GetLongitudeRadIC(),
 135                           FGIC->GetLatitudeRadIC(),
 136                           FGIC->GetAltitudeFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
 137
 138   // Set the Orientation from the euler angles
 139   VState.vQtrn = FGQuaternion( FGIC->GetPhiRadIC(),
 140                         FGIC->GetThetaRadIC(),
 141                         FGIC->GetPsiRadIC() );
 142
 143   // Set the velocities in the instantaneus body frame
 144   VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
 145                           FGIC->GetVBodyFpsIC(),
 146                           FGIC->GetWBodyFpsIC() );
 147
 148   // Set the angular velocities in the instantaneus body frame.
 149   VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
 150                           FGIC->GetQRadpsIC(),
 151                           FGIC->GetRRadpsIC() );
 152
 153   // Compute some derived values.
 154   vVel = VState.vQtrn.GetTInv()*VState.vUVW;
 155
 156   // Finally, make sure that the quaternion stays normalized.
 157   VState.vQtrn.Normalize();
 158
 159   // Recompute the RunwayRadius level.
 160   RecomputeRunwayRadius();
 161 }
 162
 163 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 164 /*
 165 Purpose: Called on a schedule to perform EOM integration
 166 Notes:   [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
 167          In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
 168
 169 At the top of this Run() function, see several "shortcuts" (or, aliases) being
 170 set up for use later, rather than using the longer class->function() notation.
 171
 172 Here, propagation of state is done using a simple explicit Euler scheme (see the
 173 bottom of the function). This propagation is done using the current state values
 174 and current derivatives. Based on these values we compute an approximation to the
 175 state values for (now + dt).
 176
 177 */
 178
 179 bool FGPropagate::Run(void)
 180 {
 181   if (FGModel::Run()) return true;  // Fast return if we have nothing to do ...
 182   if (FDMExec->Holding()) return false;
 183
 184   RecomputeRunwayRadius();
 185
 186   double dt = State->Getdt()*rate;  // The 'stepsize'
 187   const FGColumnVector3 omega( 0.0, 0.0, Inertial->omega() ); // earth rotation
 188   const FGColumnVector3& vForces = Aircraft->GetForces();     // current forces
 189   const FGColumnVector3& vMoments = Aircraft->GetMoments();   // current moments
 190
 191   double mass = MassBalance->GetMass();             // mass
 192   const FGMatrix33& J = MassBalance->GetJ();        // inertia matrix
 193   const FGMatrix33& Jinv = MassBalance->GetJinv();  // inertia matrix inverse
 194   double r = GetRadius();                           // radius
 195   if (r == 0.0) {cerr << "radius = 0 !" << endl; r = 1e-16;} // radius check
 196   double rInv = 1.0/r;
 197   FGColumnVector3 gAccel( 0.0, 0.0, Inertial->GetGAccel(r) );
 198
 199   // The rotation matrices:
 200   const FGMatrix33& Tl2b = GetTl2b();  // local to body frame
 201   const FGMatrix33& Tb2l = GetTb2l();  // body to local frame
 202   const FGMatrix33& Tec2l = VState.vLocation.GetTec2l();  // earth centered to local frame
 203   const FGMatrix33& Tl2ec = VState.vLocation.GetTl2ec();  // local to earth centered frame
 204
 205   // Inertial angular velocity measured in the body frame.
 206   const FGColumnVector3 pqri = VState.vPQR + Tl2b*(Tec2l*omega);
 207
 208   // Compute vehicle velocity wrt EC frame, expressed in Local horizontal frame.
 209   vVel = Tb2l * VState.vUVW;
 210
 211   // First compute the time derivatives of the vehicle state values:
 212
 213   // Compute body frame rotational accelerations based on the current body moments
 214   vPQRdot = Jinv*(vMoments - pqri*(J*pqri));
 215
 216   // Compute body frame accelerations based on the current body forces
 217   vUVWdot = VState.vUVW*VState.vPQR + vForces/mass;
 218
 219   // Coriolis acceleration.
 220   FGColumnVector3 ecVel = Tl2ec*vVel;
 221   FGColumnVector3 ace = 2.0*omega*ecVel;
 222   vUVWdot -= Tl2b*(Tec2l*ace);
 223
 224   if (!GroundReactions->GetWOW()) {
 225     // Centrifugal acceleration.
 226     FGColumnVector3 aeec = omega*(omega*VState.vLocation);
 227     vUVWdot -= Tl2b*(Tec2l*aeec);
 228   }
 229
 230   // Gravitation accel
 231   vUVWdot += Tl2b*gAccel;
 232
 233   // Compute vehicle velocity wrt EC frame, expressed in EC frame
 234   FGColumnVector3 vLocationDot = Tl2ec * vVel;
 235
 236   FGColumnVector3 omegaLocal( rInv*vVel(eEast),
 237                               -rInv*vVel(eNorth),
 238                               -rInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
 239
 240   // Compute quaternion orientation derivative on current body rates
 241   FGQuaternion vQtrndot = VState.vQtrn.GetQDot( VState.vPQR - Tl2b*omegaLocal );
 242
 243   // Propagate velocities
 244   VState.vPQR += dt*vPQRdot;
 245   VState.vUVW += dt*vUVWdot;
 246
 247   // Propagate positions
 248   VState.vQtrn += dt*vQtrndot;
 249   VState.vLocation += dt*vLocationDot;
 250
 251   return false;
 252 }
 253
 254 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 255
 256 void FGPropagate::RecomputeRunwayRadius(void)
 257 {
 258   // Get the runway radius.
 259   FGLocation contactloc;
 260   FGColumnVector3 dv;
 261   FGGroundCallback* gcb = FDMExec->GetGroundCallback();
 262   double t = State->Getsim_time();
 263   gcb->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
 264   RunwayRadius = contactloc.GetRadius();
 265 }
 266
 267 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 268
 269 void FGPropagate::Seth(double tt)
 270 {
 271   VState.vLocation.SetRadius( tt + SeaLevelRadius );
 272 }
 273
 274 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 275
 276 double FGPropagate::GetRunwayRadius(void) const
 277 {
 278   return RunwayRadius;
 279 }
 280
 281 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 282
 283 double FGPropagate::GetDistanceAGL(void) const
 284 {
 285   return VState.vLocation.GetRadius() - RunwayRadius;
 286 }
 287
 288 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 289
 290 void FGPropagate::SetDistanceAGL(double tt)
 291 {
 292   VState.vLocation.SetRadius( tt + RunwayRadius );
 293 }
 294
 295 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 296
 297 void FGPropagate::bind(void)
 298 {
 299   typedef double (FGPropagate::*PMF)(int) const;
 300   PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
 301
 302   PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
 303   PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
 304   PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
 305
 306   PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
 307   PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
 308   PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
 309
 310   PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
 311   PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
 312   PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
 313
 314   PropertyManager->Tie("accelerations/pdot-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRdot);
 315   PropertyManager->Tie("accelerations/qdot-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRdot);
 316   PropertyManager->Tie("accelerations/rdot-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRdot);
 317
 318   PropertyManager->Tie("accelerations/udot-fps", this, eU, (PMF)&FGPropagate::GetUVWdot);
 319   PropertyManager->Tie("accelerations/vdot-fps", this, eV, (PMF)&FGPropagate::GetUVWdot);
 320   PropertyManager->Tie("accelerations/wdot-fps", this, eW, (PMF)&FGPropagate::GetUVWdot);
 321
 322   PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::Geth, &FGPropagate::Seth, true);
 323   PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
 324   PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
 325   PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
 326   PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
 327
 328   PropertyManager->Tie("metrics/runway-radius", this, &FGPropagate::GetRunwayRadius);
 329
 330   PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 331   PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 332   PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 333
 334   PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
 335   PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
 336   PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
 337 }
 338
 339 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 340
 341 void FGPropagate::unbind(void)
 342 {
 343   PropertyManager->Untie("velocities/v-north-fps");
 344   PropertyManager->Untie("velocities/v-east-fps");
 345   PropertyManager->Untie("velocities/v-down-fps");
 346   PropertyManager->Untie("velocities/h-dot-fps");
 347   PropertyManager->Untie("velocities/u-fps");
 348   PropertyManager->Untie("velocities/v-fps");
 349   PropertyManager->Untie("velocities/w-fps");
 350   PropertyManager->Untie("velocities/p-rad_sec");
 351   PropertyManager->Untie("velocities/q-rad_sec");
 352   PropertyManager->Untie("velocities/r-rad_sec");
 353   PropertyManager->Untie("accelerations/udot-fps");
 354   PropertyManager->Untie("accelerations/vdot-fps");
 355   PropertyManager->Untie("accelerations/wdot-fps");
 356   PropertyManager->Untie("accelerations/pdot-rad_sec");
 357   PropertyManager->Untie("accelerations/qdot-rad_sec");
 358   PropertyManager->Untie("accelerations/rdot-rad_sec");
 359   PropertyManager->Untie("position/h-sl-ft");
 360   PropertyManager->Untie("position/lat-gc-rad");
 361   PropertyManager->Untie("position/long-gc-rad");
 362   PropertyManager->Untie("position/h-agl-ft");
 363   PropertyManager->Untie("position/radius-to-vehicle-ft");
 364   PropertyManager->Untie("metrics/runway-radius");
 365   PropertyManager->Untie("attitude/phi-rad");
 366   PropertyManager->Untie("attitude/theta-rad");
 367   PropertyManager->Untie("attitude/psi-rad");
 368   PropertyManager->Untie("attitude/roll-rad");
 369   PropertyManager->Untie("attitude/pitch-rad");
 370   PropertyManager->Untie("attitude/heading-true-rad");
 371 }
 372
 373 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 374 //    The bitmasked value choices are as follows:
 375 //    unset: In this case (the default) JSBSim would only print
 376 //       out the normally expected messages, essentially echoing
 377 //       the config files as they are read. If the environment
 378 //       variable is not set, debug_lvl is set to 1 internally
 379 //    0: This requests JSBSim not to output any messages
 380 //       whatsoever.
 381 //    1: This value explicity requests the normal JSBSim
 382 //       startup messages
 383 //    2: This value asks for a message to be printed out when
 384 //       a class is instantiated
 385 //    4: When this value is set, a message is displayed when a
 386 //       FGModel object executes its Run() method
 387 //    8: When this value is set, various runtime state variables
 388 //       are printed out periodically
 389 //    16: When set various parameters are sanity checked and
 390 //       a message is printed out when they go out of bounds
 391
 392 void FGPropagate::Debug(int from)
 393 {
 394   if (debug_lvl <= 0) return;
 395
 396   if (debug_lvl & 1) { // Standard console startup message output
 397     if (from == 0) { // Constructor
 398
 399     }
 400   }
 401   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 402     if (from == 0) cout << "Instantiated: FGPropagate" << endl;
 403     if (from == 1) cout << "Destroyed:    FGPropagate" << endl;
 404   }
 405   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 406   }
 407   if (debug_lvl & 8 ) { // Runtime state variables
 408   }
 409   if (debug_lvl & 16) { // Sanity checking
 410   }
 411   if (debug_lvl & 64) {
 412     if (from == 0) { // Constructor
 413       cout << IdSrc << endl;
 414       cout << IdHdr << endl;
 415     }
 416   }
 417 }
 418 }