src/FDM/JSBSim/models/FGAccelerations.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGAccelerations.cpp
   4  Author:       Jon S. Berndt
   5  Date started: 07/12/11
   6  Purpose:      Calculates derivatives of rotational and translational rates, and
   7                of the attitude quaternion.
   8  Called by:    FGFDMExec
   9
  10  ------------- Copyright (C) 2011  Jon S. Berndt (jon@jsbsim.org) -------------
  11
  12  This program is free software; you can redistribute it and/or modify it under
  13  the terms of the GNU Lesser General Public License as published by the Free Software
  14  Foundation; either version 2 of the License, or (at your option) any later
  15  version.
  16
  17  This program is distributed in the hope that it will be useful, but WITHOUT
  18  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  19  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  20  details.
  21
  22  You should have received a copy of the GNU Lesser General Public License along with
  23  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  24  Place - Suite 330, Boston, MA  02111-1307, USA.
  25
  26  Further information about the GNU Lesser General Public License can also be found on
  27  the world wide web at http://www.gnu.org.
  28
  29 FUNCTIONAL DESCRIPTION
  30 --------------------------------------------------------------------------------
  31 This class encapsulates the calculation of the derivatives of the state vectors
  32 UVW and PQR - the translational and rotational rates relative to the planet
  33 fixed frame. The derivatives relative to the inertial frame are also calculated
  34 as a side effect. Also, the derivative of the attitude quaterion is also calculated.
  35
  36 HISTORY
  37 --------------------------------------------------------------------------------
  38 07/12/11   JSB   Created
  39
  40 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  41 COMMENTS, REFERENCES,  and NOTES
  42 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  43 [1] Stevens and Lewis, "Aircraft Control and Simulation", Second edition (2004)
  44     Wiley
  45 [2] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
  46     NASA-Ames", NASA CR-2497, January 1975
  47 [3] Erin Catto, "Iterative Dynamics with Temporal Coherence", February 22, 2005
  48 [4] Mark Harris and Robert Lyle, "Spacecraft Gravitational Torques",
  49     NASA SP-8024, May 1969
  50
  51 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  52 INCLUDES
  53 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  54
  55 #include "FGAccelerations.h"
  56 #include "FGFDMExec.h"
  57 #include "input_output/FGPropertyManager.h"
  58
  59 using namespace std;
  60
  61 namespace JSBSim {
  62
  63 static const char *IdSrc = "$Id: FGAccelerations.cpp,v 1.13 2012/02/18 19:11:37 bcoconni Exp $";
  64 static const char *IdHdr = ID_ACCELERATIONS;
  65
  66 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  67 CLASS IMPLEMENTATION
  68 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  69
  70 FGAccelerations::FGAccelerations(FGFDMExec* fdmex)
  71   : FGModel(fdmex)
  72 {
  73   Debug(0);
  74   Name = "FGAccelerations";
  75   gravType = gtWGS84;
  76   gravTorque = false;
  77
  78   vPQRidot.InitMatrix();
  79   vUVWidot.InitMatrix();
  80   vGravAccel.InitMatrix();
  81   vBodyAccel.InitMatrix();
  82   vQtrndot = FGQuaternion(0,0,0);
  83
  84   bind();
  85   Debug(0);
  86 }
  87
  88 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  89
  90 FGAccelerations::~FGAccelerations(void)
  91 {
  92   Debug(1);
  93 }
  94
  95 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  96
  97 bool FGAccelerations::InitModel(void)
  98 {
  99   vPQRidot.InitMatrix();
 100   vUVWidot.InitMatrix();
 101   vGravAccel.InitMatrix();
 102   vBodyAccel.InitMatrix();
 103   vQtrndot = FGQuaternion(0,0,0);
 104
 105   return true;
 106 }
 107
 108 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 109 /*
 110 Purpose: Called on a schedule to calculate derivatives.
 111 */
 112
 113 bool FGAccelerations::Run(bool Holding)
 114 {
 115   if (FGModel::Run(Holding)) return true;  // Fast return if we have nothing to do ...
 116   if (Holding) return false;
 117
 118   CalculatePQRdot();   // Angular rate derivative
 119   CalculateUVWdot();   // Translational rate derivative
 120   CalculateQuatdot();  // Angular orientation derivative
 121
 122   ResolveFrictionForces(in.DeltaT * rate);  // Update rate derivatives with friction forces
 123
 124   Debug(2);
 125   return false;
 126 }
 127
 128 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 129 // Compute body frame rotational accelerations based on the current body moments
 130 //
 131 // vPQRdot is the derivative of the absolute angular velocity of the vehicle
 132 // (body rate with respect to the ECEF frame), expressed in the body frame,
 133 // where the derivative is taken in the body frame.
 134 // J is the inertia matrix
 135 // Jinv is the inverse inertia matrix
 136 // vMoments is the moment vector in the body frame
 137 // in.vPQRi is the total inertial angular velocity of the vehicle
 138 // expressed in the body frame.
 139 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 140 //            Second edition (2004), eqn 1.5-16e (page 50)
 141
 142 void FGAccelerations::CalculatePQRdot(void)
 143 {
 144   if (gravTorque) {
 145     // Compute the gravitational torque
 146     // Reference: See Harris and Lyle "Spacecraft Gravitational Torques",
 147     //            NASA SP-8024 (1969) eqn (2) (page 7)
 148     FGColumnVector3 R = in.Ti2b * in.vInertialPosition;
 149     double invRadius = 1.0 / R.Magnitude();
 150     R *= invRadius;
 151     in.Moment += (3.0 * in.GAccel * invRadius) * (R * (in.J * R));
 152   }
 153
 154   // Compute body frame rotational accelerations based on the current body
 155   // moments and the total inertial angular velocity expressed in the body
 156   // frame.
 157
 158   vPQRidot = in.Jinv * (in.Moment - in.vPQRi * (in.J * in.vPQRi));
 159   vPQRdot = vPQRidot - in.vPQRi * (in.Ti2b * in.vOmegaPlanet);
 160 }
 161
 162 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 163 // Compute the quaternion orientation derivative
 164 //
 165 // vQtrndot is the quaternion derivative.
 166 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 167 //            Second edition (2004), eqn 1.5-16b (page 50)
 168
 169 void FGAccelerations::CalculateQuatdot(void)
 170 {
 171   // Compute quaternion orientation derivative on current body rates
 172   vQtrndot = in.qAttitudeECI.GetQDot(in.vPQRi);
 173 }
 174
 175 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 176 // This set of calculations results in the body and inertial frame accelerations
 177 // being computed.
 178 // Compute body and inertial frames accelerations based on the current body
 179 // forces including centripetal and Coriolis accelerations for the former.
 180 // in.vOmegaPlanet is the Earth angular rate - expressed in the inertial frame -
 181 //   so it has to be transformed to the body frame. More completely,
 182 //   in.vOmegaPlanet is the rate of the ECEF frame relative to the Inertial
 183 //   frame (ECI), expressed in the Inertial frame.
 184 // in.Force is the total force on the vehicle in the body frame.
 185 // in.vPQR is the vehicle body rate relative to the ECEF frame, expressed
 186 //   in the body frame.
 187 // in.vUVW is the vehicle velocity relative to the ECEF frame, expressed
 188 //   in the body frame.
 189 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
 190 //            Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
 191
 192 void FGAccelerations::CalculateUVWdot(void)
 193 {
 194   vBodyAccel = in.Force / in.Mass;
 195
 196   vUVWdot = vBodyAccel - (in.vPQR + 2.0 * (in.Ti2b * in.vOmegaPlanet)) * in.vUVW;
 197
 198   // Include Centripetal acceleration.
 199   vUVWdot -= in.Ti2b * (in.vOmegaPlanet * (in.vOmegaPlanet * in.vInertialPosition));
 200
 201   // Include Gravitation accel
 202   switch (gravType) {
 203     case gtStandard:
 204       {
 205         double radius = in.vInertialPosition.Magnitude();
 206         vGravAccel = -(in.GAccel / radius) * in.vInertialPosition;
 207       }
 208       break;
 209     case gtWGS84:
 210       vGravAccel = in.Tec2i * in.J2Grav;
 211       break;
 212   }
 213
 214   vUVWdot += in.Ti2b * vGravAccel;
 215   vUVWidot = in.Tb2i * vBodyAccel + vGravAccel;
 216 }
 217
 218 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 219 // Resolves the contact forces just before integrating the EOM.
 220 // This routine is using Lagrange multipliers and the projected Gauss-Seidel
 221 // (PGS) method.
 222 // Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence",
 223 //            February 22, 2005
 224 // In JSBSim there is only one rigid body (the aircraft) and there can be
 225 // multiple points of contact between the aircraft and the ground. As a
 226 // consequence our matrix Jac*M^-1*Jac^T is not sparse and the algorithm
 227 // described in Catto's paper has been adapted accordingly.
 228 // The friction forces are resolved in the body frame relative to the origin
 229 // (Earth center).
 230
 231 void FGAccelerations::ResolveFrictionForces(double dt)
 232 {
 233   const double invMass = 1.0 / in.Mass;
 234   const FGMatrix33& Jinv = in.Jinv;
 235   FGColumnVector3 vdot, wdot;
 236   vector<LagrangeMultiplier*>& multipliers = *in.MultipliersList;
 237   int n = multipliers.size();
 238
 239   vFrictionForces.InitMatrix();
 240   vFrictionMoments.InitMatrix();
 241
 242   // If no gears are in contact with the ground then return
 243   if (!n) return;
 244
 245   vector<double> a(n*n); // Will contain Jac*M^-1*Jac^T
 246   vector<double> rhs(n);
 247
 248   // Assemble the linear system of equations
 249   for (int i=0; i < n; i++) {
 250     FGColumnVector3 v1 = invMass * multipliers[i]->ForceJacobian;
 251     FGColumnVector3 v2 = Jinv * multipliers[i]->MomentJacobian; // Should be J^-T but J is symmetric and so is J^-1
 252
 253     for (int j=0; j < i; j++)
 254       a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of Jac^T*M^-1*Jac
 255     for (int j=i; j < n; j++)
 256       a[i*n+j] = DotProduct(v1, multipliers[j]->ForceJacobian)
 257                + DotProduct(v2, multipliers[j]->MomentJacobian);
 258   }
 259
 260   // Assemble the RHS member
 261
 262   // Translation
 263   vdot = vUVWdot;
 264   if (dt > 0.) // Zeroes out the relative movement between the aircraft and the ground
 265     vdot += (in.vUVW - in.Tec2b * in.TerrainVelocity) / dt;
 266
 267   // Rotation
 268   wdot = vPQRdot;
 269   if (dt > 0.) // Zeroes out the relative movement between the aircraft and the ground
 270     wdot += (in.vPQR - in.Tec2b * in.TerrainAngularVel) / dt;
 271
 272   // Prepare the linear system for the Gauss-Seidel algorithm :
 273   // 1. Compute the right hand side member 'rhs'
 274   // 2. Divide every line of 'a' and 'rhs' by a[i,i]. This is in order to save
 275   //    a division computation at each iteration of Gauss-Seidel.
 276   for (int i=0; i < n; i++) {
 277     double d = 1.0 / a[i*n+i];
 278
 279     rhs[i] = -(DotProduct(multipliers[i]->ForceJacobian, vdot)
 280               +DotProduct(multipliers[i]->MomentJacobian, wdot))*d;
 281     for (int j=0; j < n; j++)
 282       a[i*n+j] *= d;
 283   }
 284
 285   // Resolve the Lagrange multipliers with the projected Gauss-Seidel method
 286   for (int iter=0; iter < 50; iter++) {
 287     double norm = 0.;
 288
 289     for (int i=0; i < n; i++) {
 290       double lambda0 = multipliers[i]->value;
 291       double dlambda = rhs[i];
 292
 293       for (int j=0; j < n; j++)
 294         dlambda -= a[i*n+j]*multipliers[j]->value;
 295
 296       multipliers[i]->value = Constrain(multipliers[i]->Min, lambda0+dlambda, multipliers[i]->Max);
 297       dlambda = multipliers[i]->value - lambda0;
 298
 299       norm += fabs(dlambda);
 300     }
 301
 302     if (norm < 1E-5) break;
 303   }
 304
 305   // Calculate the total friction forces and moments
 306
 307   for (int i=0; i< n; i++) {
 308     double lambda = multipliers[i]->value;
 309     vFrictionForces += lambda * multipliers[i]->ForceJacobian;
 310     vFrictionMoments += lambda * multipliers[i]->MomentJacobian;
 311   }
 312
 313   FGColumnVector3 accel = invMass * vFrictionForces;
 314   FGColumnVector3 omegadot = Jinv * vFrictionMoments;
 315
 316   vBodyAccel += accel;
 317   vUVWdot += accel;
 318   vUVWidot += in.Tb2i * accel;
 319   vPQRdot += omegadot;
 320   vPQRidot += omegadot;
 321 }
 322
 323 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 324
 325 void FGAccelerations::InitializeDerivatives(void)
 326 {
 327   // Make an initial run and set past values
 328   CalculatePQRdot();           // Angular rate derivative
 329   CalculateUVWdot();           // Translational rate derivative
 330   CalculateQuatdot();          // Angular orientation derivative
 331   ResolveFrictionForces(0.);   // Update rate derivatives with friction forces
 332 }
 333
 334 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 335
 336 void FGAccelerations::bind(void)
 337 {
 338   typedef double (FGAccelerations::*PMF)(int) const;
 339
 340   PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGAccelerations::GetPQRdot);
 341   PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGAccelerations::GetPQRdot);
 342   PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGAccelerations::GetPQRdot);
 343
 344   PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGAccelerations::GetUVWdot);
 345   PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGAccelerations::GetUVWdot);
 346   PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGAccelerations::GetUVWdot);
 347
 348   PropertyManager->Tie("simulation/gravity-model", &gravType);
 349   PropertyManager->Tie("simulation/gravitational-torque", &gravTorque);
 350
 351   PropertyManager->Tie("forces/fbx-total-lbs", this, eX, (PMF)&FGAccelerations::GetForces);
 352   PropertyManager->Tie("forces/fby-total-lbs", this, eY, (PMF)&FGAccelerations::GetForces);
 353   PropertyManager->Tie("forces/fbz-total-lbs", this, eZ, (PMF)&FGAccelerations::GetForces);
 354   PropertyManager->Tie("moments/l-total-lbsft", this, eL, (PMF)&FGAccelerations::GetMoments);
 355   PropertyManager->Tie("moments/m-total-lbsft", this, eM, (PMF)&FGAccelerations::GetMoments);
 356   PropertyManager->Tie("moments/n-total-lbsft", this, eN, (PMF)&FGAccelerations::GetMoments);
 357
 358   PropertyManager->Tie("moments/l-gear-lbsft", this, eL, (PMF)&FGAccelerations::GetGroundMoments);
 359   PropertyManager->Tie("moments/m-gear-lbsft", this, eM, (PMF)&FGAccelerations::GetGroundMoments);
 360   PropertyManager->Tie("moments/n-gear-lbsft", this, eN, (PMF)&FGAccelerations::GetGroundMoments);
 361   PropertyManager->Tie("forces/fbx-gear-lbs", this, eX, (PMF)&FGAccelerations::GetGroundForces);
 362   PropertyManager->Tie("forces/fby-gear-lbs", this, eY, (PMF)&FGAccelerations::GetGroundForces);
 363   PropertyManager->Tie("forces/fbz-gear-lbs", this, eZ, (PMF)&FGAccelerations::GetGroundForces);
 364 }
 365
 366 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 367 //    The bitmasked value choices are as follows:
 368 //    unset: In this case (the default) JSBSim would only print
 369 //       out the normally expected messages, essentially echoing
 370 //       the config files as they are read. If the environment
 371 //       variable is not set, debug_lvl is set to 1 internally
 372 //    0: This requests JSBSim not to output any messages
 373 //       whatsoever.
 374 //    1: This value explicity requests the normal JSBSim
 375 //       startup messages
 376 //    2: This value asks for a message to be printed out when
 377 //       a class is instantiated
 378 //    4: When this value is set, a message is displayed when a
 379 //       FGModel object executes its Run() method
 380 //    8: When this value is set, various runtime state variables
 381 //       are printed out periodically
 382 //    16: When set various parameters are sanity checked and
 383 //       a message is printed out when they go out of bounds
 384
 385 void FGAccelerations::Debug(int from)
 386 {
 387   if (debug_lvl <= 0) return;
 388
 389   if (debug_lvl & 1) { // Standard console startup message output
 390     if (from == 0) { // Constructor
 391
 392     }
 393   }
 394   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 395     if (from == 0) cout << "Instantiated: FGAccelerations" << endl;
 396     if (from == 1) cout << "Destroyed:    FGAccelerations" << endl;
 397   }
 398   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 399   }
 400   if (debug_lvl & 8 && from == 2) { // Runtime state variables
 401   }
 402   if (debug_lvl & 16) { // Sanity checking
 403   }
 404   if (debug_lvl & 64) {
 405     if (from == 0) { // Constructor
 406       cout << IdSrc << endl;
 407       cout << IdHdr << endl;
 408     }
 409   }
 410 }
 411 }