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.88 2011/05/20 03:18:36 jberndt 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 // For initialization ONLY:
125 SeaLevelRadius = LocalTerrainRadius = FDMExec->GetInertial()->GetRefRadius();
127 VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
128 VState.vLocation.SetEllipse(FDMExec->GetInertial()->GetSemimajor(), FDMExec->GetInertial()->GetSemiminor());
129 vOmegaEarth = FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->omega() ); // Earth rotation vector
131 vPQRidot.InitMatrix();
132 vQtrndot = FGQuaternion(0,0,0);
133 vUVWidot.InitMatrix();
134 vInertialVelocity.InitMatrix();
136 VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
137 VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
138 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
139 VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
141 integrator_rotational_rate = eRectEuler;
142 integrator_translational_rate = eAdamsBashforth2;
143 integrator_rotational_position = eRectEuler;
144 integrator_translational_position = eAdamsBashforth3;
149 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
151 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
153 SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
154 SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
156 // Initialize the State Vector elements and the transformation matrices
158 // Set the position lat/lon/radius
159 VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
160 FGIC->GetLatitudeRadIC(),
161 FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
163 VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
165 Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
166 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
168 VState.vInertialPosition = Tec2i * VState.vLocation;
170 UpdateLocationMatrices();
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() );
179 VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
180 UpdateBodyMatrices();
182 // Set the velocities in the instantaneus body frame
183 VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
184 FGIC->GetVBodyFpsIC(),
185 FGIC->GetWBodyFpsIC() );
187 // Compute the local frame ECEF velocity
188 vVel = Tb2l * VState.vUVW;
190 // Recompute the LocalTerrainRadius.
191 RecomputeLocalTerrainRadius();
193 VehicleRadius = GetRadius();
195 // Set the angular velocities of the body frame relative to the ECEF frame,
196 // expressed in the body frame.
197 VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
199 FGIC->GetRRadpsIC() );
201 VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
203 // Make an initial run and set past values
204 InitializeDerivatives();
207 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
209 Purpose: Called on a schedule to perform EOM integration
210 Notes: [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
211 In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
213 At the top of this Run() function, see several "shortcuts" (or, aliases) being
214 set up for use later, rather than using the longer class->function() notation.
216 This propagation is done using the current state values
217 and current derivatives. Based on these values we compute an approximation to the
218 state values for (now + dt).
220 In the code below, variables named beginning with a small "v" refer to a
221 a column vector, variables named beginning with a "T" refer to a transformation
222 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
227 bool FGPropagate::Run(bool Holding)
229 if (FGModel::Run(Holding)) return true; // Fast return if we have nothing to do ...
230 if (Holding) return false;
232 double dt = FDMExec->GetDeltaT()*rate; // The 'stepsize'
236 // Calculate state derivatives
237 CalculatePQRdot(); // Angular rate derivative
238 CalculateUVWdot(); // Translational rate derivative
239 ResolveFrictionForces(dt); // Update rate derivatives with friction forces
240 CalculateQuatdot(); // Angular orientation derivative
242 // Propagate rotational / translational velocity, angular /translational position, respectively.
244 Integrate(VState.vPQRi, vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate);
245 Integrate(VState.qAttitudeECI, vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
246 Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
247 Integrate(VState.vInertialVelocity, vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate);
249 // CAUTION : the order of the operations below is very important to get transformation
250 // matrices that are consistent with the new state of the vehicle
252 // 1. Update the Earth position angle (EPA)
253 VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
255 // 2. Update the Ti2ec and Tec2i transforms from the updated EPA
256 Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
257 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
259 // 3. Update the location from the updated Ti2ec and inertial position
260 VState.vLocation = Ti2ec*VState.vInertialPosition;
262 // 4. Update the other "Location-based" transformation matrices from the updated
264 UpdateLocationMatrices();
266 // 5. Normalize the ECI Attitude quaternion
267 VState.qAttitudeECI.Normalize();
269 // 6. Update the "Orientation-based" transformation matrices from the updated
270 // orientation quaternion and vLocation vector.
271 UpdateBodyMatrices();
273 // Translational position derivative (velocities are integrated in the inertial frame)
276 // Set auxilliary state variables
277 RecomputeLocalTerrainRadius();
279 VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
281 VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
283 VState.qAttitudeLocal = Tl2b.GetQuaternion();
285 // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
286 vVel = Tb2l * VState.vUVW;
294 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
295 // Compute body frame rotational accelerations based on the current body moments
297 // vPQRdot is the derivative of the absolute angular velocity of the vehicle
298 // (body rate with respect to the inertial frame), expressed in the body frame,
299 // where the derivative is taken in the body frame.
300 // J is the inertia matrix
301 // Jinv is the inverse inertia matrix
302 // vMoments is the moment vector in the body frame
303 // VState.vPQRi is the total inertial angular velocity of the vehicle
304 // expressed in the body frame.
305 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
306 // Second edition (2004), eqn 1.5-16e (page 50)
308 void FGPropagate::CalculatePQRdot(void)
310 const FGColumnVector3& vMoments = FDMExec->GetAircraft()->GetMoments(); // current moments
311 const FGMatrix33& J = FDMExec->GetMassBalance()->GetJ(); // inertia matrix
312 const FGMatrix33& Jinv = FDMExec->GetMassBalance()->GetJinv(); // inertia matrix inverse
314 // Compute body frame rotational accelerations based on the current body
315 // moments and the total inertial angular velocity expressed in the body
318 vPQRidot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
319 vPQRdot = vPQRidot - VState.vPQRi * (Ti2b * vOmegaEarth);
322 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
323 // Compute the quaternion orientation derivative
325 // vQtrndot is the quaternion derivative.
326 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
327 // Second edition (2004), eqn 1.5-16b (page 50)
329 void FGPropagate::CalculateQuatdot(void)
331 // Compute quaternion orientation derivative on current body rates
332 vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi);
335 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
336 // This set of calculations results in the body and inertial frame accelerations
338 // Compute body and inertial frames accelerations based on the current body
339 // forces including centripetal and coriolis accelerations for the former.
340 // vOmegaEarth is the Earth angular rate - expressed in the inertial frame -
341 // so it has to be transformed to the body frame. More completely,
342 // vOmegaEarth is the rate of the ECEF frame relative to the Inertial
343 // frame (ECI), expressed in the Inertial frame.
344 // vForces is the total force on the vehicle in the body frame.
345 // VState.vPQR is the vehicle body rate relative to the ECEF frame, expressed
346 // in the body frame.
347 // VState.vUVW is the vehicle velocity relative to the ECEF frame, expressed
348 // in the body frame.
349 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
350 // Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
352 void FGPropagate::CalculateUVWdot(void)
354 double mass = FDMExec->GetMassBalance()->GetMass(); // mass
355 const FGColumnVector3& vForces = FDMExec->GetAircraft()->GetForces(); // current forces
357 vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
359 // Include Centripetal acceleration.
360 vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
362 // Include Gravitation accel
365 vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->GetGAccel(VehicleRadius) );
368 vGravAccel = Tec2b * FDMExec->GetInertial()->GetGravityJ2(VState.vLocation);
372 vUVWdot += vGravAccel;
373 vUVWidot = Tb2i * (vForces/mass + vGravAccel);
376 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
377 // Transform the velocity vector of the body relative to the origin (Earth
378 // center) to be expressed in the inertial frame, and add the vehicle velocity
379 // contribution due to the rotation of the planet.
380 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
381 // Second edition (2004), eqn 1.5-16c (page 50)
383 void FGPropagate::CalculateInertialVelocity(void)
385 VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
388 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
389 // Transform the velocity vector of the inertial frame to be expressed in the
390 // body frame relative to the origin (Earth center), and substract the vehicle
391 // velocity contribution due to the rotation of the planet.
393 void FGPropagate::CalculateUVW(void)
395 VState.vUVW = Ti2b * (VState.vInertialVelocity - (vOmegaEarth * VState.vInertialPosition));
398 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
400 void FGPropagate::Integrate( FGColumnVector3& Integrand,
401 FGColumnVector3& Val,
402 deque <FGColumnVector3>& ValDot,
404 eIntegrateType integration_type)
406 ValDot.push_front(Val);
409 switch(integration_type) {
410 case eRectEuler: Integrand += dt*ValDot[0];
412 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
414 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
416 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
418 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
420 case eNone: // do nothing, freeze translational rate
425 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
427 void FGPropagate::Integrate( FGQuaternion& Integrand,
429 deque <FGQuaternion>& ValDot,
431 eIntegrateType integration_type)
433 ValDot.push_front(Val);
436 switch(integration_type) {
437 case eRectEuler: Integrand += dt*ValDot[0];
439 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
441 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
443 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
445 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
447 case eNone: // do nothing, freeze rotational rate
452 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
453 // Evaluates the rates (translation or rotation) that the friction forces have
454 // to resist to. This includes the external forces and moments as well as the
455 // relative movement between the aircraft and the ground.
456 // Erin Catto's paper (see ref [6]) only supports Euler integration scheme and
457 // this algorithm has been adapted to handle the multistep algorithms that
458 // JSBSim supports (i.e. Trapezoidal, Adams-Bashforth 2, 3 and 4). The capacity
459 // to handle the multistep integration schemes adds some complexity but it
460 // significantly helps stabilizing the friction forces.
462 void FGPropagate::EvaluateRateToResistTo(FGColumnVector3& vdot,
463 const FGColumnVector3& Val,
464 const FGColumnVector3& ValDot,
465 const FGColumnVector3& LocalTerrainVal,
466 deque <FGColumnVector3>& dqValDot,
468 const eIntegrateType integration_type)
470 switch(integration_type) {
471 case eAdamsBashforth4:
472 vdot = ValDot + Ti2b * (-59.*dqValDot[0]+37.*dqValDot[1]-9.*dqValDot[2])/55.;
473 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
474 vdot += 24.*(Val - Tec2b * LocalTerrainVal) / (55.*dt);
476 case eAdamsBashforth3:
477 vdot = ValDot + Ti2b * (-16.*dqValDot[0]+5.*dqValDot[1])/23.;
478 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
479 vdot += 12.*(Val - Tec2b * LocalTerrainVal) / (23.*dt);
481 case eAdamsBashforth2:
482 vdot = ValDot - Ti2b * dqValDot[0]/3.;
483 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
484 vdot += 2.*(Val - Tec2b * LocalTerrainVal) / (3.*dt);
487 vdot = ValDot + Ti2b * dqValDot[0];
488 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
489 vdot += 2.*(Val - Tec2b * LocalTerrainVal) / dt;
493 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
494 vdot += (Val - Tec2b * LocalTerrainVal) / dt;
501 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
502 // Resolves the contact forces just before integrating the EOM.
503 // This routine is using Lagrange multipliers and the projected Gauss-Seidel
505 // Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence",
507 // In JSBSim there is only one rigid body (the aircraft) and there can be
508 // multiple points of contact between the aircraft and the ground. As a
509 // consequence our matrix J*M^-1*J^T is not sparse and the algorithm described
510 // in Catto's paper has been adapted accordingly.
511 // The friction forces are resolved in the body frame relative to the origin
514 void FGPropagate::ResolveFrictionForces(double dt)
516 const double invMass = 1.0 / FDMExec->GetMassBalance()->GetMass();
517 const FGMatrix33& Jinv = FDMExec->GetMassBalance()->GetJinv();
518 vector <FGColumnVector3> JacF, JacM;
519 vector<double> lambda, lambdaMin, lambdaMax;
520 FGColumnVector3 vdot, wdot;
521 FGColumnVector3 Fc, Mc;
524 // Compiles data from the ground reactions to build up the jacobian matrix
525 for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it, n++) {
526 JacF.push_back((*it)->ForceJacobian);
527 JacM.push_back((*it)->MomentJacobian);
528 lambda.push_back((*it)->value);
529 lambdaMax.push_back((*it)->Max);
530 lambdaMin.push_back((*it)->Min);
533 // If no gears are in contact with the ground then return
536 vector<double> a(n*n); // Will contain J*M^-1*J^T
537 vector<double> rhs(n);
539 // Assemble the linear system of equations
540 for (int i=0; i < n; i++) {
541 for (int j=0; j < i; j++)
542 a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of J^T*M^-1*J
543 for (int j=i; j < n; j++)
544 a[i*n+j] = DotProduct(JacF[i],invMass*JacF[j])+DotProduct(JacM[i],Jinv*JacM[j]);
547 // Assemble the RHS member
550 EvaluateRateToResistTo(vdot, VState.vUVW, vUVWdot, LocalTerrainVelocity,
551 VState.dqUVWidot, dt, integrator_translational_rate);
554 EvaluateRateToResistTo(wdot, VState.vPQR, vPQRdot, LocalTerrainAngularVelocity,
555 VState.dqPQRidot, dt, integrator_rotational_rate);
557 // Prepare the linear system for the Gauss-Seidel algorithm :
558 // 1. Compute the right hand side member 'rhs'
559 // 2. Divide every line of 'a' and 'rhs' by a[i,i]. This is in order to save
560 // a division computation at each iteration of Gauss-Seidel.
561 for (int i=0; i < n; i++) {
562 double d = 1.0 / a[i*n+i];
564 rhs[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
565 for (int j=0; j < n; j++)
569 // Resolve the Lagrange multipliers with the projected Gauss-Seidel method
570 for (int iter=0; iter < 50; iter++) {
573 for (int i=0; i < n; i++) {
574 double lambda0 = lambda[i];
575 double dlambda = rhs[i];
577 for (int j=0; j < n; j++)
578 dlambda -= a[i*n+j]*lambda[j];
580 lambda[i] = Constrain(lambdaMin[i], lambda0+dlambda, lambdaMax[i]);
581 dlambda = lambda[i] - lambda0;
583 norm += fabs(dlambda);
586 if (norm < 1E-5) break;
589 // Calculate the total friction forces and moments
594 for (int i=0; i< n; i++) {
595 Fc += lambda[i]*JacF[i];
596 Mc += lambda[i]*JacM[i];
599 vUVWdot += invMass * Fc;
600 vUVWidot += invMass * Tb2i * Fc;
601 vPQRdot += Jinv * Mc;
602 vPQRidot += Jinv * Mc;
604 // Save the value of the Lagrange multipliers to accelerate the convergence
605 // of the Gauss-Seidel algorithm at next iteration.
607 for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it)
608 (*it)->value = lambda[i++];
610 FDMExec->GetGroundReactions()->UpdateForcesAndMoments();
613 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
615 void FGPropagate::UpdateLocationMatrices(void)
617 Tl2ec = VState.vLocation.GetTl2ec(); // local to ECEF transform
618 Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
619 Ti2l = VState.vLocation.GetTi2l(); // ECI to local frame transform
620 Tl2i = Ti2l.Transposed(); // local to ECI transform
623 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
625 void FGPropagate::UpdateBodyMatrices(void)
627 Ti2b = VState.qAttitudeECI.GetT(); // ECI to body frame transform
628 Tb2i = Ti2b.Transposed(); // body to ECI frame transform
629 Tl2b = Ti2b * Tl2i; // local to body frame transform
630 Tb2l = Tl2b.Transposed(); // body to local frame transform
631 Tec2b = Ti2b * Tec2i; // ECEF to body frame transform
632 Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
635 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
637 void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
638 VState.qAttitudeECI = Qi;
639 VState.qAttitudeECI.Normalize();
640 UpdateBodyMatrices();
641 VState.qAttitudeLocal = Tl2b.GetQuaternion();
644 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
646 void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
647 VState.vInertialVelocity = Vi;
649 vVel = Tb2l * VState.vUVW;
652 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
654 void FGPropagate::SetInertialRates(FGColumnVector3 vRates) {
655 VState.vPQRi = Ti2b * vRates;
656 VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
659 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
661 void FGPropagate::InitializeDerivatives(void)
663 // Make an initial run and set past values
664 CalculatePQRdot(); // Angular rate derivative
665 CalculateUVWdot(); // Translational rate derivative
666 ResolveFrictionForces(0.); // Update rate derivatives with friction forces
667 CalculateQuatdot(); // Angular orientation derivative
668 CalculateInertialVelocity(); // Translational position derivative
670 // Initialize past values deques
671 VState.dqPQRidot.clear();
672 VState.dqUVWidot.clear();
673 VState.dqInertialVelocity.clear();
674 VState.dqQtrndot.clear();
675 for (int i=0; i<4; i++) {
676 VState.dqPQRidot.push_front(vPQRidot);
677 VState.dqUVWidot.push_front(vUVWidot);
678 VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
679 VState.dqQtrndot.push_front(vQtrndot);
683 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
685 void FGPropagate::RecomputeLocalTerrainRadius(void)
687 FGLocation contactloc;
689 double t = FDMExec->GetSimTime();
691 // Get the LocalTerrain radius.
692 FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv,
693 LocalTerrainVelocity, LocalTerrainAngularVelocity);
694 LocalTerrainRadius = contactloc.GetRadius();
697 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
699 void FGPropagate::SetTerrainElevation(double terrainElev)
701 LocalTerrainRadius = terrainElev + SeaLevelRadius;
702 FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
705 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
707 double FGPropagate::GetTerrainElevation(void) const
709 return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
712 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
714 double FGPropagate::GetDistanceAGL(void) const
716 return VState.vLocation.GetRadius() - LocalTerrainRadius;
719 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
721 void FGPropagate::SetVState(const VehicleState& vstate)
723 VState.vLocation = vstate.vLocation;
724 VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
725 Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
726 Tec2i = Ti2ec.Transposed();
727 UpdateLocationMatrices();
728 SetInertialOrientation(vstate.qAttitudeECI);
729 RecomputeLocalTerrainRadius();
730 VehicleRadius = GetRadius();
731 VState.vUVW = vstate.vUVW;
732 vVel = Tb2l * VState.vUVW;
733 VState.vPQR = vstate.vPQR;
734 VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
735 VState.vInertialPosition = vstate.vInertialPosition;
737 InitializeDerivatives();
740 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
742 void FGPropagate::UpdateVehicleState(void)
744 RecomputeLocalTerrainRadius();
745 VehicleRadius = GetRadius();
746 VState.vInertialPosition = Tec2i * VState.vLocation;
747 UpdateLocationMatrices();
748 UpdateBodyMatrices();
749 vVel = Tb2l * VState.vUVW;
750 VState.qAttitudeLocal = Tl2b.GetQuaternion();
753 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
755 void FGPropagate::SetLocation(const FGLocation& l)
757 VState.vLocation = l;
758 VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
759 Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
760 Tec2i = Ti2ec.Transposed();
761 UpdateVehicleState();
764 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
766 void FGPropagate::DumpState(void)
770 << "------------------------------------------------------------------" << reset << endl;
772 << "State Report at sim time: " << FDMExec->GetSimTime() << " seconds" << reset << endl;
773 cout << " " << underon
774 << "Position" << underoff << endl;
775 cout << " ECI: " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl;
776 cout << " ECEF: " << VState.vLocation << " (x,y,z, in ft)" << endl;
777 cout << " Local: " << VState.vLocation.GetLatitudeDeg()
778 << ", " << VState.vLocation.GetLongitudeDeg()
779 << ", " << GetAltitudeASL() << " (lat, lon, alt in deg and ft)" << endl;
781 cout << endl << " " << underon
782 << "Orientation" << underoff << endl;
783 cout << " ECI: " << VState.qAttitudeECI.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
784 cout << " Local: " << VState.qAttitudeLocal.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
786 cout << endl << " " << underon
787 << "Velocity" << underoff << endl;
788 cout << " ECI: " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl;
789 cout << " ECEF: " << (Tb2ec * VState.vUVW).Dump(", ") << " (x,y,z in ft/s)" << endl;
790 cout << " Local: " << GetVel() << " (n,e,d in ft/sec)" << endl;
791 cout << " Body: " << GetUVW() << " (u,v,w in ft/sec)" << endl;
793 cout << endl << " " << underon
794 << "Body Rates (relative to given frame, expressed in body frame)" << underoff << endl;
795 cout << " ECI: " << (VState.vPQRi*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
796 cout << " ECEF: " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
799 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
801 void FGPropagate::bind(void)
803 typedef double (FGPropagate::*PMF)(int) const;
805 PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
807 PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
808 PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
809 PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
811 PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
812 PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
813 PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
815 PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
816 PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
817 PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
819 PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
820 PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
821 PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
823 PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
825 PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
826 PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
827 PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
829 PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
830 PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
831 PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
833 PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
834 PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
835 PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
836 PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
837 PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
838 PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
839 PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
840 PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
841 PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
842 PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
843 PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
844 PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
845 &FGPropagate::GetTerrainElevation,
846 &FGPropagate::SetTerrainElevation, false);
848 PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
850 PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
851 PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
852 PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
854 PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
855 PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
856 PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
858 PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
859 PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
860 PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
861 PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
862 PropertyManager->Tie("simulation/gravity-model", &gravType);
865 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
866 // The bitmasked value choices are as follows:
867 // unset: In this case (the default) JSBSim would only print
868 // out the normally expected messages, essentially echoing
869 // the config files as they are read. If the environment
870 // variable is not set, debug_lvl is set to 1 internally
871 // 0: This requests JSBSim not to output any messages
873 // 1: This value explicity requests the normal JSBSim
875 // 2: This value asks for a message to be printed out when
876 // a class is instantiated
877 // 4: When this value is set, a message is displayed when a
878 // FGModel object executes its Run() method
879 // 8: When this value is set, various runtime state variables
880 // are printed out periodically
881 // 16: When set various parameters are sanity checked and
882 // a message is printed out when they go out of bounds
884 void FGPropagate::Debug(int from)
886 if (debug_lvl <= 0) return;
888 if (debug_lvl & 1) { // Standard console startup message output
889 if (from == 0) { // Constructor
893 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
894 if (from == 0) cout << "Instantiated: FGPropagate" << endl;
895 if (from == 1) cout << "Destroyed: FGPropagate" << endl;
897 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
899 if (debug_lvl & 8 && from == 2) { // Runtime state variables
900 cout << endl << fgblue << highint << left
901 << " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
904 cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
905 << FDMExec->GetInertial()->GetEarthPositionAngleDeg() << endl;
907 cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
908 cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
909 cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
910 cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
911 cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
912 cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
913 cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
914 cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
916 cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
917 << reset << endl << Tec2b.Dump("\t", " ") << endl;
918 cout << highint << " Associated Euler angles (deg): " << setw(8)
919 << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
922 cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
923 << reset << endl << Tb2ec.Dump("\t", " ") << endl;
924 cout << highint << " Associated Euler angles (deg): " << setw(8)
925 << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
928 cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
929 << reset << endl << Tl2b.Dump("\t", " ") << endl;
930 cout << highint << " Associated Euler angles (deg): " << setw(8)
931 << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
934 cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
935 << reset << endl << Tb2l.Dump("\t", " ") << endl;
936 cout << highint << " Associated Euler angles (deg): " << setw(8)
937 << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
940 cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
941 << reset << endl << Tl2ec.Dump("\t", " ") << endl;
942 cout << highint << " Associated Euler angles (deg): " << setw(8)
943 << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
946 cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
947 << reset << endl << Tec2l.Dump("\t", " ") << endl;
948 cout << highint << " Associated Euler angles (deg): " << setw(8)
949 << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
952 cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
953 << reset << endl << Tec2i.Dump("\t", " ") << endl;
954 cout << highint << " Associated Euler angles (deg): " << setw(8)
955 << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
958 cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
959 << reset << endl << Ti2ec.Dump("\t", " ") << endl;
960 cout << highint << " Associated Euler angles (deg): " << setw(8)
961 << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
964 cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
965 << reset << endl << Ti2b.Dump("\t", " ") << endl;
966 cout << highint << " Associated Euler angles (deg): " << setw(8)
967 << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
970 cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
971 << reset << endl << Tb2i.Dump("\t", " ") << endl;
972 cout << highint << " Associated Euler angles (deg): " << setw(8)
973 << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
976 cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
977 << reset << endl << Ti2l.Dump("\t", " ") << endl;
978 cout << highint << " Associated Euler angles (deg): " << setw(8)
979 << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
982 cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
983 << reset << endl << Tl2i.Dump("\t", " ") << endl;
984 cout << highint << " Associated Euler angles (deg): " << setw(8)
985 << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
988 cout << setprecision(6); // reset the output stream
990 if (debug_lvl & 16) { // Sanity checking
991 if (from == 2) { // State sanity checking
992 if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
993 cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
996 if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
997 cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
1000 if (fabs(GetDistanceAGL()) > 1e10) {
1001 cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
1006 if (debug_lvl & 64) {
1007 if (from == 0) { // Constructor
1008 cout << IdSrc << endl;
1009 cout << IdHdr << endl;