1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3 Module: FGPropagate.cpp
6 Purpose: Integrate the EOM to determine instantaneous position
9 ------------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) -------------
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
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
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.
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.
28 FUNCTIONAL DESCRIPTION
29 --------------------------------------------------------------------------------
30 This class encapsulates the integration of rates and accelerations to get the
31 current position of the aircraft.
34 --------------------------------------------------------------------------------
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
43 [2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
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
53 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
55 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
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"
74 static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.86 2011/04/17 11:27:14 bcoconni Exp $";
75 static const char *IdHdr = ID_PROPAGATE;
77 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
79 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
81 FGPropagate::FGPropagate(FGFDMExec* fdmex)
83 LocalTerrainRadius(0),
91 vPQRidot.InitMatrix();
92 vQtrndot = FGQuaternion(0,0,0);
93 vUVWidot.InitMatrix();
94 vInertialVelocity.InitMatrix();
96 /// These define the indices use to select the various integrators.
97 // eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
99 integrator_rotational_rate = eRectEuler;
100 integrator_translational_rate = eAdamsBashforth2;
101 integrator_rotational_position = eRectEuler;
102 integrator_translational_position = eAdamsBashforth3;
104 VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
105 VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
106 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
107 VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0));
113 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
115 FGPropagate::~FGPropagate(void)
120 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
122 bool FGPropagate::InitModel(void)
124 if (!FGModel::InitModel()) return false;
126 // For initialization ONLY:
127 SeaLevelRadius = LocalTerrainRadius = FDMExec->GetInertial()->GetRefRadius();
129 VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
130 VState.vLocation.SetEllipse(FDMExec->GetInertial()->GetSemimajor(), FDMExec->GetInertial()->GetSemiminor());
131 vOmegaEarth = FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->omega() ); // Earth rotation vector
133 vPQRidot.InitMatrix();
134 vQtrndot = FGQuaternion(0,0,0);
135 vUVWidot.InitMatrix();
136 vInertialVelocity.InitMatrix();
138 VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
139 VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
140 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
141 VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
143 integrator_rotational_rate = eAdamsBashforth2;
144 integrator_translational_rate = eTrapezoidal;
145 integrator_rotational_position = eAdamsBashforth2;
146 integrator_translational_position = eTrapezoidal;
151 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
153 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
155 SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
156 SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
158 // Initialize the State Vector elements and the transformation matrices
160 // Set the position lat/lon/radius
161 VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
162 FGIC->GetLatitudeRadIC(),
163 FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
165 VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
167 Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
168 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
170 VState.vInertialPosition = Tec2i * VState.vLocation;
172 UpdateLocationMatrices();
174 // Set the orientation from the euler angles (is normalized within the
175 // constructor). The Euler angles represent the orientation of the body
176 // frame relative to the local frame.
177 VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(),
178 FGIC->GetThetaRadIC(),
179 FGIC->GetPsiRadIC() );
181 VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
182 UpdateBodyMatrices();
184 // Set the velocities in the instantaneus body frame
185 VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
186 FGIC->GetVBodyFpsIC(),
187 FGIC->GetWBodyFpsIC() );
189 // Compute the local frame ECEF velocity
190 vVel = Tb2l * VState.vUVW;
192 // Recompute the LocalTerrainRadius.
193 RecomputeLocalTerrainRadius();
195 VehicleRadius = GetRadius();
197 // Set the angular velocities of the body frame relative to the ECEF frame,
198 // expressed in the body frame.
199 VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
201 FGIC->GetRRadpsIC() );
203 VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
205 // Make an initial run and set past values
206 InitializeDerivatives();
209 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
211 Purpose: Called on a schedule to perform EOM integration
212 Notes: [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
213 In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
215 At the top of this Run() function, see several "shortcuts" (or, aliases) being
216 set up for use later, rather than using the longer class->function() notation.
218 This propagation is done using the current state values
219 and current derivatives. Based on these values we compute an approximation to the
220 state values for (now + dt).
222 In the code below, variables named beginning with a small "v" refer to a
223 a column vector, variables named beginning with a "T" refer to a transformation
224 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
229 bool FGPropagate::Run(void)
231 if (FGModel::Run()) return true; // Fast return if we have nothing to do ...
232 if (FDMExec->Holding()) return false;
234 double dt = FDMExec->GetDeltaT()*rate; // The 'stepsize'
238 // Calculate state derivatives
239 CalculatePQRdot(); // Angular rate derivative
240 CalculateUVWdot(); // Translational rate derivative
241 ResolveFrictionForces(dt); // Update rate derivatives with friction forces
242 CalculateQuatdot(); // Angular orientation derivative
244 // Propagate rotational / translational velocity, angular /translational position, respectively.
246 Integrate(VState.vPQRi, vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate);
247 Integrate(VState.qAttitudeECI, vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
248 Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
249 Integrate(VState.vInertialVelocity, vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate);
251 // CAUTION : the order of the operations below is very important to get transformation
252 // matrices that are consistent with the new state of the vehicle
254 // 1. Update the Earth position angle (EPA)
255 VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
257 // 2. Update the Ti2ec and Tec2i transforms from the updated EPA
258 Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
259 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
261 // 3. Update the location from the updated Ti2ec and inertial position
262 VState.vLocation = Ti2ec*VState.vInertialPosition;
264 // 4. Update the other "Location-based" transformation matrices from the updated
266 UpdateLocationMatrices();
268 // 5. Normalize the ECI Attitude quaternion
269 VState.qAttitudeECI.Normalize();
271 // 6. Update the "Orientation-based" transformation matrices from the updated
272 // orientation quaternion and vLocation vector.
273 UpdateBodyMatrices();
275 // Translational position derivative (velocities are integrated in the inertial frame)
278 // Set auxilliary state variables
279 RecomputeLocalTerrainRadius();
281 VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
283 VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
285 VState.qAttitudeLocal = Tl2b.GetQuaternion();
287 // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
288 vVel = Tb2l * VState.vUVW;
296 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
297 // Compute body frame rotational accelerations based on the current body moments
299 // vPQRdot is the derivative of the absolute angular velocity of the vehicle
300 // (body rate with respect to the inertial frame), expressed in the body frame,
301 // where the derivative is taken in the body frame.
302 // J is the inertia matrix
303 // Jinv is the inverse inertia matrix
304 // vMoments is the moment vector in the body frame
305 // VState.vPQRi is the total inertial angular velocity of the vehicle
306 // expressed in the body frame.
307 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
308 // Second edition (2004), eqn 1.5-16e (page 50)
310 void FGPropagate::CalculatePQRdot(void)
312 const FGColumnVector3& vMoments = FDMExec->GetAircraft()->GetMoments(); // current moments
313 const FGMatrix33& J = FDMExec->GetMassBalance()->GetJ(); // inertia matrix
314 const FGMatrix33& Jinv = FDMExec->GetMassBalance()->GetJinv(); // inertia matrix inverse
316 // Compute body frame rotational accelerations based on the current body
317 // moments and the total inertial angular velocity expressed in the body
320 vPQRidot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
321 vPQRdot = vPQRidot - VState.vPQRi * (Ti2b * vOmegaEarth);
324 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
325 // Compute the quaternion orientation derivative
327 // vQtrndot is the quaternion derivative.
328 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
329 // Second edition (2004), eqn 1.5-16b (page 50)
331 void FGPropagate::CalculateQuatdot(void)
333 // Compute quaternion orientation derivative on current body rates
334 vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi);
337 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
338 // This set of calculations results in the body and inertial frame accelerations
340 // Compute body and inertial frames accelerations based on the current body
341 // forces including centripetal and coriolis accelerations for the former.
342 // vOmegaEarth is the Earth angular rate - expressed in the inertial frame -
343 // so it has to be transformed to the body frame. More completely,
344 // vOmegaEarth is the rate of the ECEF frame relative to the Inertial
345 // frame (ECI), expressed in the Inertial frame.
346 // vForces is the total force on the vehicle in the body frame.
347 // VState.vPQR is the vehicle body rate relative to the ECEF frame, expressed
348 // in the body frame.
349 // VState.vUVW is the vehicle velocity relative to the ECEF frame, expressed
350 // in the body frame.
351 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
352 // Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
354 void FGPropagate::CalculateUVWdot(void)
356 double mass = FDMExec->GetMassBalance()->GetMass(); // mass
357 const FGColumnVector3& vForces = FDMExec->GetAircraft()->GetForces(); // current forces
359 vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
361 // Include Centripetal acceleration.
362 vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
364 // Include Gravitation accel
367 vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->GetGAccel(VehicleRadius) );
370 vGravAccel = Tec2b * FDMExec->GetInertial()->GetGravityJ2(VState.vLocation);
374 vUVWdot += vGravAccel;
375 vUVWidot = Tb2i * (vForces/mass + vGravAccel);
378 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
379 // Transform the velocity vector of the body relative to the origin (Earth
380 // center) to be expressed in the inertial frame, and add the vehicle velocity
381 // contribution due to the rotation of the planet.
382 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
383 // Second edition (2004), eqn 1.5-16c (page 50)
385 void FGPropagate::CalculateInertialVelocity(void)
387 VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
390 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
391 // Transform the velocity vector of the inertial frame to be expressed in the
392 // body frame relative to the origin (Earth center), and substract the vehicle
393 // velocity contribution due to the rotation of the planet.
395 void FGPropagate::CalculateUVW(void)
397 VState.vUVW = Ti2b * (VState.vInertialVelocity - (vOmegaEarth * VState.vInertialPosition));
400 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
402 void FGPropagate::Integrate( FGColumnVector3& Integrand,
403 FGColumnVector3& Val,
404 deque <FGColumnVector3>& ValDot,
406 eIntegrateType integration_type)
408 ValDot.push_front(Val);
411 switch(integration_type) {
412 case eRectEuler: Integrand += dt*ValDot[0];
414 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
416 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
418 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
420 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
422 case eNone: // do nothing, freeze translational rate
427 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
429 void FGPropagate::Integrate( FGQuaternion& Integrand,
431 deque <FGQuaternion>& ValDot,
433 eIntegrateType integration_type)
435 ValDot.push_front(Val);
438 switch(integration_type) {
439 case eRectEuler: Integrand += dt*ValDot[0];
441 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
443 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
445 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
447 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
449 case eNone: // do nothing, freeze rotational rate
454 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
455 // Evaluates the rates (translation or rotation) that the friction forces have
456 // to resist to. This includes the external forces and moments as well as the
457 // relative movement between the aircraft and the ground.
458 // Erin Catto's paper (see ref [6]) only supports Euler integration scheme and
459 // this algorithm has been adapted to handle the multistep algorithms that
460 // JSBSim supports (i.e. Trapezoidal, Adams-Bashforth 2, 3 and 4). The capacity
461 // to handle the multistep integration schemes adds some complexity but it
462 // significantly helps stabilizing the friction forces.
464 void FGPropagate::EvaluateRateToResistTo(FGColumnVector3& vdot,
465 const FGColumnVector3& Val,
466 const FGColumnVector3& ValDot,
467 const FGColumnVector3& LocalTerrainVal,
468 deque <FGColumnVector3>& dqValDot,
470 const eIntegrateType integration_type)
472 switch(integration_type) {
473 case eAdamsBashforth4:
474 vdot = ValDot + Ti2b * (-59.*dqValDot[0]+37.*dqValDot[1]-9.*dqValDot[2])/55.;
475 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
476 vdot += 24.*(Val - Tec2b * LocalTerrainVal) / (55.*dt);
478 case eAdamsBashforth3:
479 vdot = ValDot + Ti2b * (-16.*dqValDot[0]+5.*dqValDot[1])/23.;
480 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
481 vdot += 12.*(Val - Tec2b * LocalTerrainVal) / (23.*dt);
483 case eAdamsBashforth2:
484 vdot = ValDot - Ti2b * dqValDot[0]/3.;
485 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
486 vdot += 2.*(Val - Tec2b * LocalTerrainVal) / (3.*dt);
489 vdot = ValDot + Ti2b * dqValDot[0];
490 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
491 vdot += 2.*(Val - Tec2b * LocalTerrainVal) / dt;
495 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
496 vdot += (Val - Tec2b * LocalTerrainVal) / dt;
503 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
504 // Resolves the contact forces just before integrating the EOM.
505 // This routine is using Lagrange multipliers and the projected Gauss-Seidel
507 // Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence",
509 // In JSBSim there is only one rigid body (the aircraft) and there can be
510 // multiple points of contact between the aircraft and the ground. As a
511 // consequence our matrix J*M^-1*J^T is not sparse and the algorithm described
512 // in Catto's paper has been adapted accordingly.
513 // The friction forces are resolved in the body frame relative to the origin
516 void FGPropagate::ResolveFrictionForces(double dt)
518 const double invMass = 1.0 / FDMExec->GetMassBalance()->GetMass();
519 const FGMatrix33& Jinv = FDMExec->GetMassBalance()->GetJinv();
520 vector <FGColumnVector3> JacF, JacM;
521 vector<double> lambda, lambdaMin, lambdaMax;
522 FGColumnVector3 vdot, wdot;
523 FGColumnVector3 Fc, Mc;
526 // Compiles data from the ground reactions to build up the jacobian matrix
527 for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it, n++) {
528 JacF.push_back((*it)->ForceJacobian);
529 JacM.push_back((*it)->MomentJacobian);
530 lambda.push_back((*it)->value);
531 lambdaMax.push_back((*it)->Max);
532 lambdaMin.push_back((*it)->Min);
535 // If no gears are in contact with the ground then return
538 vector<double> a(n*n); // Will contain J*M^-1*J^T
539 vector<double> rhs(n);
541 // Assemble the linear system of equations
542 for (int i=0; i < n; i++) {
543 for (int j=0; j < i; j++)
544 a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of J^T*M^-1*J
545 for (int j=i; j < n; j++)
546 a[i*n+j] = DotProduct(JacF[i],invMass*JacF[j])+DotProduct(JacM[i],Jinv*JacM[j]);
549 // Assemble the RHS member
552 EvaluateRateToResistTo(vdot, VState.vUVW, vUVWdot, LocalTerrainVelocity,
553 VState.dqUVWidot, dt, integrator_translational_rate);
556 EvaluateRateToResistTo(wdot, VState.vPQR, vPQRdot, LocalTerrainAngularVelocity,
557 VState.dqPQRidot, dt, integrator_rotational_rate);
559 // Prepare the linear system for the Gauss-Seidel algorithm :
560 // 1. Compute the right hand side member 'rhs'
561 // 2. Divide every line of 'a' and 'rhs' by a[i,i]. This is in order to save
562 // a division computation at each iteration of Gauss-Seidel.
563 for (int i=0; i < n; i++) {
564 double d = 1.0 / a[i*n+i];
566 rhs[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
567 for (int j=0; j < n; j++)
571 // Resolve the Lagrange multipliers with the projected Gauss-Seidel method
572 for (int iter=0; iter < 50; iter++) {
575 for (int i=0; i < n; i++) {
576 double lambda0 = lambda[i];
577 double dlambda = rhs[i];
579 for (int j=0; j < n; j++)
580 dlambda -= a[i*n+j]*lambda[j];
582 lambda[i] = Constrain(lambdaMin[i], lambda0+dlambda, lambdaMax[i]);
583 dlambda = lambda[i] - lambda0;
585 norm += fabs(dlambda);
588 if (norm < 1E-5) break;
591 // Calculate the total friction forces and moments
596 for (int i=0; i< n; i++) {
597 Fc += lambda[i]*JacF[i];
598 Mc += lambda[i]*JacM[i];
601 vUVWdot += invMass * Fc;
602 vUVWidot += invMass * Tb2i * Fc;
603 vPQRdot += Jinv * Mc;
604 vPQRidot += Jinv * Mc;
606 // Save the value of the Lagrange multipliers to accelerate the convergence
607 // of the Gauss-Seidel algorithm at next iteration.
609 for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it)
610 (*it)->value = lambda[i++];
612 FDMExec->GetGroundReactions()->UpdateForcesAndMoments();
615 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
617 void FGPropagate::UpdateLocationMatrices(void)
619 Tl2ec = VState.vLocation.GetTl2ec(); // local to ECEF transform
620 Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
621 Ti2l = VState.vLocation.GetTi2l(); // ECI to local frame transform
622 Tl2i = Ti2l.Transposed(); // local to ECI transform
625 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
627 void FGPropagate::UpdateBodyMatrices(void)
629 Ti2b = VState.qAttitudeECI.GetT(); // ECI to body frame transform
630 Tb2i = Ti2b.Transposed(); // body to ECI frame transform
631 Tl2b = Ti2b * Tl2i; // local to body frame transform
632 Tb2l = Tl2b.Transposed(); // body to local frame transform
633 Tec2b = Ti2b * Tec2i; // ECEF to body frame transform
634 Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
637 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
639 void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
640 VState.qAttitudeECI = Qi;
641 VState.qAttitudeECI.Normalize();
642 UpdateBodyMatrices();
643 VState.qAttitudeLocal = Tl2b.GetQuaternion();
646 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
648 void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
649 VState.vInertialVelocity = Vi;
651 vVel = Tb2l * VState.vUVW;
654 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
656 void FGPropagate::SetInertialRates(FGColumnVector3 vRates) {
657 VState.vPQRi = Ti2b * vRates;
658 VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
661 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
663 void FGPropagate::InitializeDerivatives(void)
665 // Make an initial run and set past values
666 CalculatePQRdot(); // Angular rate derivative
667 CalculateUVWdot(); // Translational rate derivative
668 ResolveFrictionForces(0.); // Update rate derivatives with friction forces
669 CalculateQuatdot(); // Angular orientation derivative
670 CalculateInertialVelocity(); // Translational position derivative
672 // Initialize past values deques
673 VState.dqPQRidot.clear();
674 VState.dqUVWidot.clear();
675 VState.dqInertialVelocity.clear();
676 VState.dqQtrndot.clear();
677 for (int i=0; i<4; i++) {
678 VState.dqPQRidot.push_front(vPQRidot);
679 VState.dqUVWidot.push_front(vUVWidot);
680 VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
681 VState.dqQtrndot.push_front(vQtrndot);
685 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
687 void FGPropagate::RecomputeLocalTerrainRadius(void)
689 FGLocation contactloc;
691 double t = FDMExec->GetSimTime();
693 // Get the LocalTerrain radius.
694 FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv,
695 LocalTerrainVelocity, LocalTerrainAngularVelocity);
696 LocalTerrainRadius = contactloc.GetRadius();
699 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
701 void FGPropagate::SetTerrainElevation(double terrainElev)
703 LocalTerrainRadius = terrainElev + SeaLevelRadius;
704 FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
707 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
709 double FGPropagate::GetTerrainElevation(void) const
711 return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
714 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
716 double FGPropagate::GetDistanceAGL(void) const
718 return VState.vLocation.GetRadius() - LocalTerrainRadius;
721 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
723 void FGPropagate::SetVState(const VehicleState& vstate)
725 VState.vLocation = vstate.vLocation;
726 VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
727 Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
728 Tec2i = Ti2ec.Transposed();
729 UpdateLocationMatrices();
730 SetInertialOrientation(vstate.qAttitudeECI);
731 RecomputeLocalTerrainRadius();
732 VehicleRadius = GetRadius();
733 VState.vUVW = vstate.vUVW;
734 vVel = Tb2l * VState.vUVW;
735 VState.vPQR = vstate.vPQR;
736 VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
737 VState.vInertialPosition = vstate.vInertialPosition;
739 InitializeDerivatives();
742 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
744 void FGPropagate::UpdateVehicleState(void)
746 RecomputeLocalTerrainRadius();
747 VehicleRadius = GetRadius();
748 VState.vInertialPosition = Tec2i * VState.vLocation;
749 UpdateLocationMatrices();
750 UpdateBodyMatrices();
751 vVel = Tb2l * VState.vUVW;
752 VState.qAttitudeLocal = Tl2b.GetQuaternion();
755 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
757 void FGPropagate::SetLocation(const FGLocation& l)
759 VState.vLocation = l;
760 VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
761 Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
762 Tec2i = Ti2ec.Transposed();
763 UpdateVehicleState();
766 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
768 void FGPropagate::DumpState(void)
772 << "------------------------------------------------------------------" << reset << endl;
774 << "State Report at sim time: " << FDMExec->GetSimTime() << " seconds" << reset << endl;
775 cout << " " << underon
776 << "Position" << underoff << endl;
777 cout << " ECI: " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl;
778 cout << " ECEF: " << VState.vLocation << " (x,y,z, in ft)" << endl;
779 cout << " Local: " << VState.vLocation.GetLatitudeDeg()
780 << ", " << VState.vLocation.GetLongitudeDeg()
781 << ", " << GetAltitudeASL() << " (lat, lon, alt in deg and ft)" << endl;
783 cout << endl << " " << underon
784 << "Orientation" << underoff << endl;
785 cout << " ECI: " << VState.qAttitudeECI.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
786 cout << " Local: " << VState.qAttitudeLocal.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
788 cout << endl << " " << underon
789 << "Velocity" << underoff << endl;
790 cout << " ECI: " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl;
791 cout << " ECEF: " << (Tb2ec * VState.vUVW).Dump(", ") << " (x,y,z in ft/s)" << endl;
792 cout << " Local: " << GetVel() << " (n,e,d in ft/sec)" << endl;
793 cout << " Body: " << GetUVW() << " (u,v,w in ft/sec)" << endl;
795 cout << endl << " " << underon
796 << "Body Rates (relative to given frame, expressed in body frame)" << underoff << endl;
797 cout << " ECI: " << (VState.vPQRi*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
798 cout << " ECEF: " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
801 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
803 void FGPropagate::bind(void)
805 typedef double (FGPropagate::*PMF)(int) const;
807 PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
809 PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
810 PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
811 PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
813 PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
814 PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
815 PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
817 PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
818 PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
819 PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
821 PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
822 PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
823 PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
825 PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
827 PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
828 PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
829 PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
831 PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
832 PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
833 PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
835 PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
836 PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
837 PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
838 PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
839 PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
840 PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
841 PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
842 PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
843 PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
844 PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
845 PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
846 PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
847 &FGPropagate::GetTerrainElevation,
848 &FGPropagate::SetTerrainElevation, false);
850 PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
852 PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
853 PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
854 PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
856 PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
857 PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
858 PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
860 PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
861 PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
862 PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
863 PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
864 PropertyManager->Tie("simulation/gravity-model", &gravType);
867 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
868 // The bitmasked value choices are as follows:
869 // unset: In this case (the default) JSBSim would only print
870 // out the normally expected messages, essentially echoing
871 // the config files as they are read. If the environment
872 // variable is not set, debug_lvl is set to 1 internally
873 // 0: This requests JSBSim not to output any messages
875 // 1: This value explicity requests the normal JSBSim
877 // 2: This value asks for a message to be printed out when
878 // a class is instantiated
879 // 4: When this value is set, a message is displayed when a
880 // FGModel object executes its Run() method
881 // 8: When this value is set, various runtime state variables
882 // are printed out periodically
883 // 16: When set various parameters are sanity checked and
884 // a message is printed out when they go out of bounds
886 void FGPropagate::Debug(int from)
888 if (debug_lvl <= 0) return;
890 if (debug_lvl & 1) { // Standard console startup message output
891 if (from == 0) { // Constructor
895 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
896 if (from == 0) cout << "Instantiated: FGPropagate" << endl;
897 if (from == 1) cout << "Destroyed: FGPropagate" << endl;
899 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
901 if (debug_lvl & 8 && from == 2) { // Runtime state variables
902 cout << endl << fgblue << highint << left
903 << " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
906 cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
907 << FDMExec->GetInertial()->GetEarthPositionAngleDeg() << endl;
909 cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
910 cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
911 cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
912 cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
913 cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
914 cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
915 cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
916 cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
918 cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
919 << reset << endl << Tec2b.Dump("\t", " ") << endl;
920 cout << highint << " Associated Euler angles (deg): " << setw(8)
921 << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
924 cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
925 << reset << endl << Tb2ec.Dump("\t", " ") << endl;
926 cout << highint << " Associated Euler angles (deg): " << setw(8)
927 << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
930 cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
931 << reset << endl << Tl2b.Dump("\t", " ") << endl;
932 cout << highint << " Associated Euler angles (deg): " << setw(8)
933 << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
936 cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
937 << reset << endl << Tb2l.Dump("\t", " ") << endl;
938 cout << highint << " Associated Euler angles (deg): " << setw(8)
939 << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
942 cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
943 << reset << endl << Tl2ec.Dump("\t", " ") << endl;
944 cout << highint << " Associated Euler angles (deg): " << setw(8)
945 << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
948 cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
949 << reset << endl << Tec2l.Dump("\t", " ") << endl;
950 cout << highint << " Associated Euler angles (deg): " << setw(8)
951 << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
954 cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
955 << reset << endl << Tec2i.Dump("\t", " ") << endl;
956 cout << highint << " Associated Euler angles (deg): " << setw(8)
957 << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
960 cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
961 << reset << endl << Ti2ec.Dump("\t", " ") << endl;
962 cout << highint << " Associated Euler angles (deg): " << setw(8)
963 << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
966 cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
967 << reset << endl << Ti2b.Dump("\t", " ") << endl;
968 cout << highint << " Associated Euler angles (deg): " << setw(8)
969 << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
972 cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
973 << reset << endl << Tb2i.Dump("\t", " ") << endl;
974 cout << highint << " Associated Euler angles (deg): " << setw(8)
975 << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
978 cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
979 << reset << endl << Ti2l.Dump("\t", " ") << endl;
980 cout << highint << " Associated Euler angles (deg): " << setw(8)
981 << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
984 cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
985 << reset << endl << Tl2i.Dump("\t", " ") << endl;
986 cout << highint << " Associated Euler angles (deg): " << setw(8)
987 << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
990 cout << setprecision(6); // reset the output stream
992 if (debug_lvl & 16) { // Sanity checking
993 if (from == 2) { // State sanity checking
994 if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
995 cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
998 if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
999 cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
1002 if (fabs(GetDistanceAGL()) > 1e10) {
1003 cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
1008 if (debug_lvl & 64) {
1009 if (from == 0) { // Constructor
1010 cout << IdSrc << endl;
1011 cout << IdHdr << endl;