]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/models/FGPropagate.cpp
9c7543c48f7ab45fb4a284ef293426800ab52cae
[flightgear.git] / 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 "FGAircraft.h"
66 #include "FGMassBalance.h"
67 #include "FGInertial.h"
68 #include "input_output/FGPropertyManager.h"
69
70 using namespace std;
71
72 namespace JSBSim {
73
74 static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.59 2010/07/30 11:50:01 jberndt Exp $";
75 static const char *IdHdr = ID_PROPAGATE;
76
77 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
78 CLASS IMPLEMENTATION
79 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
80
81 FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
82 {
83   Debug(0);
84   Name = "FGPropagate";
85   gravType = gtWGS84;
86  
87   vPQRdot.InitMatrix();
88   vQtrndot = FGQuaternion(0,0,0);
89   vUVWdot.InitMatrix();
90   vInertialVelocity.InitMatrix();
91
92   integrator_rotational_rate = eAdamsBashforth2;
93   integrator_translational_rate = eTrapezoidal;
94   integrator_rotational_position = eAdamsBashforth2;
95   integrator_translational_position = eTrapezoidal;
96
97   VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
98   VState.dqUVWdot.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   if (!FGModel::InitModel()) return false;
118
119   // For initialization ONLY:
120   SeaLevelRadius = LocalTerrainRadius = Inertial->GetRefRadius();
121
122   VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
123   VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
124   vOmegaEarth = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
125
126   vPQRdot.InitMatrix();
127   vQtrndot = FGQuaternion(0,0,0);
128   vUVWdot.InitMatrix();
129   vInertialVelocity.InitMatrix();
130
131   VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
132   VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
133   VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
134   VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
135
136   integrator_rotational_rate = eAdamsBashforth2;
137   integrator_translational_rate = eTrapezoidal;
138   integrator_rotational_position = eAdamsBashforth2;
139   integrator_translational_position = eTrapezoidal;
140
141   return true;
142 }
143
144 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
145
146 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
147 {
148   SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
149   SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
150
151   VehicleRadius = GetRadius();
152   radInv = 1.0/VehicleRadius;
153
154   // Initialize the State Vector elements and the transformation matrices
155
156   // Set the position lat/lon/radius
157   VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
158                                 FGIC->GetLatitudeRadIC(),
159                                 FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
160
161   VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle());
162
163   Ti2ec = GetTi2ec();         // ECI to ECEF transform
164   Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
165   
166   Tl2ec = GetTl2ec();         // local to ECEF transform
167   Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
168
169   Ti2l  = GetTi2l();
170   Tl2i  = Ti2l.Transposed();
171
172   // Set the orientation from the euler angles (is normalized within the
173   // constructor). The Euler angles represent the orientation of the body
174   // frame relative to the local frame.
175   VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(),
176                                         FGIC->GetThetaRadIC(),
177                                         FGIC->GetPsiRadIC() );
178
179   VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
180
181   Ti2b  = GetTi2b();           // ECI to body frame transform
182   Tb2i  = Ti2b.Transposed();   // body to ECI frame transform
183
184   Tl2b  = VState.qAttitudeLocal; // local to body frame transform
185   Tb2l  = Tl2b.Transposed();     // body to local frame transform
186
187   Tec2b = Tl2b * Tec2l;        // ECEF to body frame transform
188   Tb2ec = Tec2b.Transposed();  // body to ECEF frame tranform
189
190   // Set the velocities in the instantaneus body frame
191   VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
192                                  FGIC->GetVBodyFpsIC(),
193                                  FGIC->GetWBodyFpsIC() );
194
195   VState.vInertialPosition = Tec2i * VState.vLocation;
196
197   // Compute the local frame ECEF velocity
198   vVel = Tb2l * VState.vUVW;
199
200   // Refer to Stevens and Lewis, 1.5-14a, pg. 49.
201   // This is the rotation rate of the "Local" frame, expressed in the local frame.
202
203   FGColumnVector3 vOmegaLocal = FGColumnVector3(
204      radInv*vVel(eEast),
205     -radInv*vVel(eNorth),
206     -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
207
208   // Set the angular velocities of the body frame relative to the ECEF frame,
209   // expressed in the body frame. Effectively, this is:
210   //   w_b/e = w_b/l + w_l/e
211   VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
212                                  FGIC->GetQRadpsIC(),
213                                  FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal;
214
215   VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
216
217   // Make an initial run and set past values
218   CalculatePQRdot();           // Angular rate derivative
219   CalculateUVWdot();           // Translational rate derivative
220   ResolveFrictionForces(0.);   // Update rate derivatives with friction forces
221   CalculateQuatdot();          // Angular orientation derivative
222   CalculateInertialVelocity(); // Translational position derivative
223
224   // Initialize past values deques
225   VState.dqPQRdot.clear();
226   VState.dqUVWdot.clear();
227   VState.dqInertialVelocity.clear();
228   VState.dqQtrndot.clear();
229   for (int i=0; i<4; i++) {
230     VState.dqPQRdot.push_front(vPQRdot);
231     VState.dqUVWdot.push_front(vUVWdot);
232     VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
233     VState.dqQtrndot.push_front(vQtrndot);
234   }
235
236   // Recompute the LocalTerrainRadius.
237   RecomputeLocalTerrainRadius();
238 }
239
240 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
241 /*
242 Purpose: Called on a schedule to perform EOM integration
243 Notes:   [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
244          In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
245
246 At the top of this Run() function, see several "shortcuts" (or, aliases) being
247 set up for use later, rather than using the longer class->function() notation.
248
249 This propagation is done using the current state values
250 and current derivatives. Based on these values we compute an approximation to the
251 state values for (now + dt).
252
253 In the code below, variables named beginning with a small "v" refer to a 
254 a column vector, variables named beginning with a "T" refer to a transformation
255 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
256 Inertial.
257
258 */
259
260 bool FGPropagate::Run(void)
261 {
262 static int ctr;
263
264   if (FGModel::Run()) return true;  // Fast return if we have nothing to do ...
265   if (FDMExec->Holding()) return false;
266
267   double dt = FDMExec->GetDeltaT()*rate;  // The 'stepsize'
268
269   RunPreFunctions();
270
271   // Calculate state derivatives
272   CalculatePQRdot();           // Angular rate derivative
273   CalculateUVWdot();           // Translational rate derivative
274   ResolveFrictionForces(dt);   // Update rate derivatives with friction forces
275   CalculateQuatdot();          // Angular orientation derivative
276   CalculateInertialVelocity(); // Translational position derivative
277
278   // Propagate rotational / translational velocity, angular /translational position, respectively.
279   Integrate(VState.vPQRi,             vPQRdot,           VState.dqPQRdot,           dt, integrator_rotational_rate);
280   Integrate(VState.vUVW,              vUVWdot,           VState.dqUVWdot,           dt, integrator_translational_rate);
281   Integrate(VState.qAttitudeECI,      vQtrndot,          VState.dqQtrndot,          dt, integrator_rotational_position);
282   Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
283
284   VState.qAttitudeECI.Normalize(); // Normalize the ECI Attitude quaternion
285
286   VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle()); // Update the Earth position angle (EPA)
287
288   // Update the "Location-based" transformation matrices from the vLocation vector.
289
290   Ti2ec = GetTi2ec();          // ECI to ECEF transform
291   Tec2i = Ti2ec.Transposed();  // ECEF to ECI frame transform
292   Tl2ec = GetTl2ec();          // local to ECEF transform
293   Tec2l = Tl2ec.Transposed();  // ECEF to local frame transform
294   Ti2l  = GetTi2l();
295   Tl2i  = Ti2l.Transposed();
296
297   // Update the "Orientation-based" transformation matrices from the orientation quaternion
298
299   Ti2b  = GetTi2b();           // ECI to body frame transform
300   Tb2i  = Ti2b.Transposed();   // body to ECI frame transform
301   Tl2b  = Ti2b*Tl2i;           // local to body frame transform
302   Tb2l  = Tl2b.Transposed();   // body to local frame transform
303   Tec2b = Tl2b * Tec2l;        // ECEF to body frame transform
304   Tb2ec = Tec2b.Transposed();  // body to ECEF frame tranform
305
306   // Set auxililary state variables
307   VState.vLocation = Ti2ec*VState.vInertialPosition;
308   RecomputeLocalTerrainRadius();
309
310   VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
311   radInv = 1.0/VehicleRadius;
312
313   VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
314
315   VState.qAttitudeLocal = Tl2b.GetQuaternion();
316
317   // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
318   vVel = Tb2l * VState.vUVW;
319
320   RunPostFunctions();
321
322   Debug(2);
323   return false;
324 }
325
326 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
327 // Compute body frame rotational accelerations based on the current body moments
328 //
329 // vPQRdot is the derivative of the absolute angular velocity of the vehicle 
330 // (body rate with respect to the inertial frame), expressed in the body frame,
331 // where the derivative is taken in the body frame.
332 // J is the inertia matrix
333 // Jinv is the inverse inertia matrix
334 // vMoments is the moment vector in the body frame
335 // VState.vPQRi is the total inertial angular velocity of the vehicle
336 // expressed in the body frame.
337 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation", 
338 //            Second edition (2004), eqn 1.5-16e (page 50)
339
340 void FGPropagate::CalculatePQRdot(void)
341 {
342   const FGColumnVector3& vMoments = Aircraft->GetMoments(); // current moments
343   const FGMatrix33& J = MassBalance->GetJ();                // inertia matrix
344   const FGMatrix33& Jinv = MassBalance->GetJinv();          // inertia matrix inverse
345
346   // Compute body frame rotational accelerations based on the current body
347   // moments and the total inertial angular velocity expressed in the body
348   // frame.
349
350   vPQRdot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
351 }
352
353 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
354 // Compute the quaternion orientation derivative
355 //
356 // vQtrndot is the quaternion derivative.
357 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation", 
358 //            Second edition (2004), eqn 1.5-16b (page 50)
359
360 void FGPropagate::CalculateQuatdot(void)
361 {
362   // Compute quaternion orientation derivative on current body rates
363   vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi);
364 }
365
366 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
367 // This set of calculations results in the body frame accelerations being
368 // computed.
369 // Compute body frame accelerations based on the current body forces.
370 // Include centripetal and coriolis accelerations.
371 // vOmegaEarth is the Earth angular rate - expressed in the inertial frame -
372 //   so it has to be transformed to the body frame. More completely,
373 //   vOmegaEarth is the rate of the ECEF frame relative to the Inertial
374 //   frame (ECI), expressed in the Inertial frame.
375 // vForces is the total force on the vehicle in the body frame.
376 // VState.vPQR is the vehicle body rate relative to the ECEF frame, expressed
377 //   in the body frame.
378 // VState.vUVW is the vehicle velocity relative to the ECEF frame, expressed
379 //   in the body frame.
380 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation", 
381 //            Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
382
383 void FGPropagate::CalculateUVWdot(void)
384 {
385   double mass = MassBalance->GetMass();                      // mass
386   const FGColumnVector3& vForces = Aircraft->GetForces();    // current forces
387
388   vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
389
390   // Include Centripetal acceleration.
391   vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
392
393   // Include Gravitation accel
394   switch (gravType) {
395     case gtStandard:
396       vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) );
397       break;
398     case gtWGS84:
399       vGravAccel = Tec2b * Inertial->GetGravityJ2(VState.vLocation);
400       break;
401   }
402
403   vUVWdot += vGravAccel;
404 }
405
406 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
407   // Transform the velocity vector of the body relative to the origin (Earth
408   // center) to be expressed in the inertial frame, and add the vehicle velocity
409   // contribution due to the rotation of the planet.
410   // Reference: See Stevens and Lewis, "Aircraft Control and Simulation", 
411   //            Second edition (2004), eqn 1.5-16c (page 50)
412
413 void FGPropagate::CalculateInertialVelocity(void)
414 {
415   VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
416 }
417
418 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
419
420 void FGPropagate::Integrate( FGColumnVector3& Integrand,
421                              FGColumnVector3& Val,
422                              deque <FGColumnVector3>& ValDot,
423                              double dt,
424                              eIntegrateType integration_type)
425 {
426   ValDot.push_front(Val);
427   ValDot.pop_back();
428
429   switch(integration_type) {
430   case eRectEuler:       Integrand += dt*ValDot[0];
431     break;
432   case eTrapezoidal:     Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
433     break;
434   case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
435     break;
436   case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
437     break;
438   case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
439     break;
440   case eNone: // do nothing, freeze translational rate
441     break;
442   }
443 }
444
445 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
446
447 void FGPropagate::Integrate( FGQuaternion& Integrand,
448                              FGQuaternion& Val,
449                              deque <FGQuaternion>& ValDot,
450                              double dt,
451                              eIntegrateType integration_type)
452 {
453   ValDot.push_front(Val);
454   ValDot.pop_back();
455
456   switch(integration_type) {
457   case eRectEuler:       Integrand += dt*ValDot[0];
458     break;
459   case eTrapezoidal:     Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
460     break;
461   case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
462     break;
463   case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
464     break;
465   case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
466     break;
467   case eNone: // do nothing, freeze rotational rate
468     break;
469   }
470 }
471
472 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
473 // Resolves the contact forces just before integrating the EOM.
474 // This routine is using Lagrange multipliers and the projected Gauss-Seidel
475 // (PGS) method.
476 // Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence", 
477 //            February 22, 2005
478 // In JSBSim there is only one rigid body (the aircraft) and there can be
479 // multiple points of contact between the aircraft and the ground. As a
480 // consequence our matrix J*M^-1*J^T is not sparse and the algorithm described
481 // in Catto's paper has been adapted accordingly.
482
483 void FGPropagate::ResolveFrictionForces(double dt)
484 {
485   const double invMass = 1.0 / MassBalance->GetMass();
486   const FGMatrix33& Jinv = MassBalance->GetJinv();
487   vector <FGColumnVector3> JacF, JacM;
488   FGColumnVector3 vdot, wdot;
489   FGColumnVector3 Fc, Mc;
490   int n = 0, i;
491
492   // Compiles data from the ground reactions to build up the jacobian matrix
493   for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, n++) {
494     JacF.push_back((*it)->ForceJacobian);
495     JacM.push_back((*it)->MomentJacobian);
496   }
497
498   // If no gears are in contact with the ground then return
499   if (!n) return;
500
501   double *a = new double[n*n]; // Will contain J*M^-1*J^T
502   double *eta = new double[n];
503   double *lambda = new double[n];
504   double *lambdaMin = new double[n];
505   double *lambdaMax = new double[n];
506
507   // Initializes the Lagrange multipliers
508   i = 0;
509   for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, i++) {
510     lambda[i] = (*it)->value;
511     lambdaMax[i] = (*it)->Max;
512     lambdaMin[i] = (*it)->Min;
513   }
514
515   vdot = vUVWdot;
516   wdot = vPQRdot;
517
518   if (dt > 0.) {
519     // First compute the ground velocity below the aircraft center of gravity
520     FGLocation contact;
521     FGColumnVector3 normal, cvel;
522     double t = FDMExec->GetSimTime();
523     double height = FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contact, normal, cvel);
524
525     // Instruct the algorithm to zero out the relative movement between the
526     // aircraft and the ground.
527     vdot += (VState.vUVW - Tec2b * cvel) / dt;
528     wdot += VState.vPQR / dt;
529   }
530
531   // Assemble the linear system of equations
532   for (i=0; i < n; i++) {
533     for (int j=0; j < i; j++)
534       a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of J^T*M^-1*J
535     for (int j=i; j < n; j++)
536       a[i*n+j] = DotProduct(JacF[i],invMass*JacF[j])+DotProduct(JacM[i],Jinv*JacM[j]);
537   }
538
539   // Prepare the linear system for the Gauss-Seidel algorithm :
540   // divide every line of 'a' and eta by a[i,i]. This is in order to save
541   // a division computation at each iteration of Gauss-Seidel.
542   for (i=0; i < n; i++) {
543     double d = 1.0 / a[i*n+i];
544
545     eta[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
546     for (int j=0; j < n; j++)
547       a[i*n+j] *= d;
548   }
549
550   // Resolve the Lagrange multipliers with the projected Gauss-Seidel method
551   for (int iter=0; iter < 50; iter++) {
552     double norm = 0.;
553
554     for (i=0; i < n; i++) {
555       double lambda0 = lambda[i];
556       double dlambda = eta[i];
557       
558       for (int j=0; j < n; j++)
559         dlambda -= a[i*n+j]*lambda[j];
560
561       lambda[i] = Constrain(lambdaMin[i], lambda0+dlambda, lambdaMax[i]);
562       dlambda = lambda[i] - lambda0;
563
564       norm += fabs(dlambda);
565     }
566
567     if (norm < 1E-5) break;
568   }
569
570   // Calculate the total friction forces and moments
571
572   Fc.InitMatrix();
573   Mc.InitMatrix();
574
575   for (i=0; i< n; i++) {
576     Fc += lambda[i]*JacF[i];
577     Mc += lambda[i]*JacM[i];
578   }
579
580   vUVWdot += invMass * Fc;
581   vPQRdot += Jinv * Mc;
582
583   // Save the value of the Lagrange multipliers to accelerate the convergence
584   // of the Gauss-Seidel algorithm at next iteration.
585   i = 0;
586   for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it)
587     (*it)->value = lambda[i++];
588
589   GroundReactions->UpdateForcesAndMoments();
590
591   delete a, eta, lambda, lambdaMin, lambdaMax;
592 }
593
594 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
595
596 void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
597   VState.qAttitudeECI = Qi;
598 }
599
600 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
601
602 void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
603   VState.vInertialVelocity = Vi;
604 }
605
606 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
607
608 void FGPropagate::RecomputeLocalTerrainRadius(void)
609 {
610   double t = FDMExec->GetSimTime();
611
612   // Get the LocalTerrain radius.
613   FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
614   LocalTerrainRadius = contactloc.GetRadius(); 
615 }
616
617 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
618
619 void FGPropagate::SetTerrainElevation(double terrainElev)
620 {
621   LocalTerrainRadius = terrainElev + SeaLevelRadius;
622   FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
623 }
624
625 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
626
627 double FGPropagate::GetTerrainElevation(void) const
628 {
629   return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
630 }
631
632 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
633 //Todo: when should this be called - when should the new EPA be used to
634 // calculate the transformation matrix, so that the matrix is not a step
635 // ahead of the sim and the associated calculations?
636 const FGMatrix33& FGPropagate::GetTi2ec(void)
637 {
638   return VState.vLocation.GetTi2ec();
639 }
640
641 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
642
643 const FGMatrix33& FGPropagate::GetTec2i(void)
644 {
645   return VState.vLocation.GetTec2i();
646 }
647
648 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
649
650 void FGPropagate::SetAltitudeASL(double altASL)
651 {
652   VState.vLocation.SetRadius( altASL + SeaLevelRadius );
653 }
654
655 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
656
657 double FGPropagate::GetLocalTerrainRadius(void) const
658 {
659   return LocalTerrainRadius;
660 }
661
662 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
663
664 double FGPropagate::GetDistanceAGL(void) const
665 {
666   return VState.vLocation.GetRadius() - LocalTerrainRadius;
667 }
668
669 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
670
671 void FGPropagate::SetDistanceAGL(double tt)
672 {
673   VState.vLocation.SetRadius( tt + LocalTerrainRadius );
674 }
675
676 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
677
678 void FGPropagate::bind(void)
679 {
680   typedef double (FGPropagate::*PMF)(int) const;
681
682   PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
683
684   PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
685   PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
686   PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
687
688   PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
689   PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
690   PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
691
692   PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
693   PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
694   PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
695
696   PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
697   PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
698   PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
699
700   PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
701
702   PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
703   PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
704   PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
705
706   PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
707   PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
708   PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
709
710   PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
711   PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
712   PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
713   PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
714   PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
715   PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
716   PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
717   PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
718   PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
719   PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
720   PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
721   PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
722                           &FGPropagate::GetTerrainElevation,
723                           &FGPropagate::SetTerrainElevation, false);
724
725   PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
726
727   PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
728   PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
729   PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
730
731   PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
732   PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
733   PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
734   
735   PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
736   PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
737   PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
738   PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
739   PropertyManager->Tie("simulation/gravity-model", &gravType);
740 }
741
742 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
743 //    The bitmasked value choices are as follows:
744 //    unset: In this case (the default) JSBSim would only print
745 //       out the normally expected messages, essentially echoing
746 //       the config files as they are read. If the environment
747 //       variable is not set, debug_lvl is set to 1 internally
748 //    0: This requests JSBSim not to output any messages
749 //       whatsoever.
750 //    1: This value explicity requests the normal JSBSim
751 //       startup messages
752 //    2: This value asks for a message to be printed out when
753 //       a class is instantiated
754 //    4: When this value is set, a message is displayed when a
755 //       FGModel object executes its Run() method
756 //    8: When this value is set, various runtime state variables
757 //       are printed out periodically
758 //    16: When set various parameters are sanity checked and
759 //       a message is printed out when they go out of bounds
760
761 void FGPropagate::Debug(int from)
762 {
763   if (debug_lvl <= 0) return;
764
765   if (debug_lvl & 1) { // Standard console startup message output
766     if (from == 0) { // Constructor
767
768     }
769   }
770   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
771     if (from == 0) cout << "Instantiated: FGPropagate" << endl;
772     if (from == 1) cout << "Destroyed:    FGPropagate" << endl;
773   }
774   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
775   }
776   if (debug_lvl & 8 && from == 2) { // Runtime state variables
777     cout << endl << fgblue << highint << left 
778          << "  Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
779          << reset << endl;
780     cout << endl;
781     cout << highint << "  Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
782                     << Inertial->GetEarthPositionAngleDeg() << endl;
783     cout << endl;
784     cout << highint << "  Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
785     cout << highint << "  Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
786     cout << highint << "  Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
787     cout << highint << "  Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
788     cout << highint << "  Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
789     cout << highint << "  Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
790     cout << highint << "  Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
791     cout << highint << "  Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
792     cout << endl;
793     cout << highint << "  Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
794                     << reset << endl << Tec2b.Dump("\t", "    ") << endl;
795     cout << highint << "    Associated Euler angles (deg): " << setw(8)
796                     << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
797                     << endl << endl;
798
799     cout << highint << "  Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
800                     << reset << endl << Tb2ec.Dump("\t", "    ") << endl;
801     cout << highint << "    Associated Euler angles (deg): " << setw(8)
802                     << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
803                     << endl << endl;
804
805     cout << highint << "  Matrix Local to Body (Orientation of Body with respect to Local):"
806                     << reset << endl << Tl2b.Dump("\t", "    ") << endl;
807     cout << highint << "    Associated Euler angles (deg): " << setw(8)
808                     << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
809                     << endl << endl;
810
811     cout << highint << "  Matrix Body to Local (Orientation of Local with respect to Body):"
812                     << reset << endl << Tb2l.Dump("\t", "    ") << endl;
813     cout << highint << "    Associated Euler angles (deg): " << setw(8)
814                     << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
815                     << endl << endl;
816
817     cout << highint << "  Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
818                     << reset << endl << Tl2ec.Dump("\t", "    ") << endl;
819     cout << highint << "    Associated Euler angles (deg): " << setw(8)
820                     << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
821                     << endl << endl;
822
823     cout << highint << "  Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
824                     << reset << endl << Tec2l.Dump("\t", "    ") << endl;
825     cout << highint << "    Associated Euler angles (deg): " << setw(8)
826                     << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
827                     << endl << endl;
828
829     cout << highint << "  Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
830                     << reset << endl << Tec2i.Dump("\t", "    ") << endl;
831     cout << highint << "    Associated Euler angles (deg): " << setw(8)
832                     << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
833                     << endl << endl;
834
835     cout << highint << "  Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
836                     << reset << endl << Ti2ec.Dump("\t", "    ") << endl;
837     cout << highint << "    Associated Euler angles (deg): " << setw(8)
838                     << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
839                     << endl << endl;
840
841     cout << highint << "  Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
842                     << reset << endl << Ti2b.Dump("\t", "    ") << endl;
843     cout << highint << "    Associated Euler angles (deg): " << setw(8)
844                     << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
845                     << endl << endl;
846
847     cout << highint << "  Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
848                     << reset << endl << Tb2i.Dump("\t", "    ") << endl;
849     cout << highint << "    Associated Euler angles (deg): " << setw(8)
850                     << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
851                     << endl << endl;
852
853     cout << highint << "  Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
854                     << reset << endl << Ti2l.Dump("\t", "    ") << endl;
855     cout << highint << "    Associated Euler angles (deg): " << setw(8)
856                     << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
857                     << endl << endl;
858
859     cout << highint << "  Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
860                     << reset << endl << Tl2i.Dump("\t", "    ") << endl;
861     cout << highint << "    Associated Euler angles (deg): " << setw(8)
862                     << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
863                     << endl << endl;
864
865     cout << setprecision(6); // reset the output stream
866   }
867   if (debug_lvl & 16) { // Sanity checking
868     if (from == 2) { // State sanity checking
869       if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
870         cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
871         exit(-1);
872       }
873       if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
874         cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
875         exit(-1);
876       }
877       if (fabs(GetDistanceAGL()) > 1e10) {
878         cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
879         exit(-1);
880       }
881     }
882   }
883   if (debug_lvl & 64) {
884     if (from == 0) { // Constructor
885       cout << IdSrc << endl;
886       cout << IdHdr << endl;
887     }
888   }
889 }
890 }