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.74 2010/11/28 13:02:43 bcoconni Exp $";
75 static const char *IdHdr = ID_PROPAGATE;
77 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
79 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
81 FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex),
82 LocalTerrainRadius(0), SeaLevelRadius(0), VehicleRadius(0)
89 vQtrndot = FGQuaternion(0,0,0);
91 vInertialVelocity.InitMatrix();
93 integrator_rotational_rate = eAdamsBashforth2;
94 integrator_translational_rate = eTrapezoidal;
95 integrator_rotational_position = eAdamsBashforth2;
96 integrator_translational_position = eTrapezoidal;
98 VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
99 VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
100 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
101 VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0));
107 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
109 FGPropagate::~FGPropagate(void)
114 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
116 bool FGPropagate::InitModel(void)
118 if (!FGModel::InitModel()) return false;
120 // For initialization ONLY:
121 SeaLevelRadius = LocalTerrainRadius = FDMExec->GetInertial()->GetRefRadius();
123 VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
124 VState.vLocation.SetEllipse(FDMExec->GetInertial()->GetSemimajor(), FDMExec->GetInertial()->GetSemiminor());
125 vOmegaEarth = FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->omega() ); // Earth rotation vector
127 vPQRdot.InitMatrix();
128 vQtrndot = FGQuaternion(0,0,0);
129 vUVWdot.InitMatrix();
130 vInertialVelocity.InitMatrix();
132 VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
133 VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
134 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
135 VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
137 integrator_rotational_rate = eAdamsBashforth2;
138 integrator_translational_rate = eTrapezoidal;
139 integrator_rotational_position = eAdamsBashforth2;
140 integrator_translational_position = eTrapezoidal;
145 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
147 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
149 SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
150 SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
152 // Initialize the State Vector elements and the transformation matrices
154 // Set the position lat/lon/radius
155 VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
156 FGIC->GetLatitudeRadIC(),
157 FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
159 VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
161 Ti2ec = GetTi2ec(); // ECI to ECEF transform
162 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
164 VState.vInertialPosition = Tec2i * VState.vLocation;
166 UpdateLocationMatrices();
168 // Set the orientation from the euler angles (is normalized within the
169 // constructor). The Euler angles represent the orientation of the body
170 // frame relative to the local frame.
171 VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(),
172 FGIC->GetThetaRadIC(),
173 FGIC->GetPsiRadIC() );
175 VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
176 UpdateBodyMatrices();
178 // Set the velocities in the instantaneus body frame
179 VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
180 FGIC->GetVBodyFpsIC(),
181 FGIC->GetWBodyFpsIC() );
183 // Compute the local frame ECEF velocity
184 vVel = Tb2l * VState.vUVW;
186 // Recompute the LocalTerrainRadius.
187 RecomputeLocalTerrainRadius();
189 VehicleRadius = GetRadius();
190 double radInv = 1.0/VehicleRadius;
192 // Refer to Stevens and Lewis, 1.5-14a, pg. 49.
193 // This is the rotation rate of the "Local" frame, expressed in the local frame.
195 FGColumnVector3 vOmegaLocal = FGColumnVector3(
197 -radInv*vVel(eNorth),
198 -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
200 // Set the angular velocities of the body frame relative to the ECEF frame,
201 // expressed in the body frame. Effectively, this is:
202 // w_b/e = w_b/l + w_l/e
203 VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
205 FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal;
207 VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
208 VState.vPQRi_i = Tb2i * VState.vPQRi;
210 // Make an initial run and set past values
211 InitializeDerivatives();
214 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
216 Purpose: Called on a schedule to perform EOM integration
217 Notes: [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
218 In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
220 At the top of this Run() function, see several "shortcuts" (or, aliases) being
221 set up for use later, rather than using the longer class->function() notation.
223 This propagation is done using the current state values
224 and current derivatives. Based on these values we compute an approximation to the
225 state values for (now + dt).
227 In the code below, variables named beginning with a small "v" refer to a
228 a column vector, variables named beginning with a "T" refer to a transformation
229 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
234 bool FGPropagate::Run(void)
236 if (FGModel::Run()) return true; // Fast return if we have nothing to do ...
237 if (FDMExec->Holding()) return false;
239 double dt = FDMExec->GetDeltaT()*rate; // The 'stepsize'
243 // Calculate state derivatives
244 CalculatePQRdot(); // Angular rate derivative
245 CalculateUVWdot(); // Translational rate derivative
246 ResolveFrictionForces(dt); // Update rate derivatives with friction forces
247 CalculateQuatdot(); // Angular orientation derivative
248 CalculateUVW(); // Translational position derivative (velocities are integrated in the inertial frame)
250 // Propagate rotational / translational velocity, angular /translational position, respectively.
252 Integrate(VState.vPQRi_i, vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate); // ECI integration
253 Integrate(VState.qAttitudeECI, vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
254 Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
255 Integrate(VState.vInertialVelocity, vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate);
257 // CAUTION : the order of the operations below is very important to get transformation
258 // matrices that are consistent with the new state of the vehicle
260 // 1. Update the Earth position angle (EPA)
261 VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
263 // 2. Update the Ti2ec and Tec2i transforms from the updated EPA
264 Ti2ec = GetTi2ec(); // ECI to ECEF transform
265 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
267 // 3. Update the location from the updated Ti2ec and inertial position
268 VState.vLocation = Ti2ec*VState.vInertialPosition;
270 // 4. Update the other "Location-based" transformation matrices from the updated
272 UpdateLocationMatrices();
274 // 5. Normalize the ECI Attitude quaternion
275 VState.qAttitudeECI.Normalize();
277 // 6. Update the "Orientation-based" transformation matrices from the updated
278 // orientation quaternion and vLocation vector.
279 UpdateBodyMatrices();
281 // Set auxililary state variables
282 RecomputeLocalTerrainRadius();
284 VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
286 VState.vPQRi = Ti2b * VState.vPQRi_i;
287 VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
289 VState.qAttitudeLocal = Tl2b.GetQuaternion();
291 // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
292 vVel = Tb2l * VState.vUVW;
300 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
301 // Compute body frame rotational accelerations based on the current body moments
303 // vPQRdot is the derivative of the absolute angular velocity of the vehicle
304 // (body rate with respect to the inertial frame), expressed in the body frame,
305 // where the derivative is taken in the body frame.
306 // J is the inertia matrix
307 // Jinv is the inverse inertia matrix
308 // vMoments is the moment vector in the body frame
309 // VState.vPQRi is the total inertial angular velocity of the vehicle
310 // expressed in the body frame.
311 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
312 // Second edition (2004), eqn 1.5-16e (page 50)
314 void FGPropagate::CalculatePQRdot(void)
316 const FGColumnVector3& vMoments = FDMExec->GetAircraft()->GetMoments(); // current moments
317 const FGMatrix33& J = FDMExec->GetMassBalance()->GetJ(); // inertia matrix
318 const FGMatrix33& Jinv = FDMExec->GetMassBalance()->GetJinv(); // inertia matrix inverse
320 // Compute body frame rotational accelerations based on the current body
321 // moments and the total inertial angular velocity expressed in the body
324 vPQRdot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
325 vPQRidot = Tb2i * vPQRdot;
328 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
329 // Compute the quaternion orientation derivative
331 // vQtrndot is the quaternion derivative.
332 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
333 // Second edition (2004), eqn 1.5-16b (page 50)
335 void FGPropagate::CalculateQuatdot(void)
337 // Compute quaternion orientation derivative on current body rates
338 vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi);
341 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
342 // This set of calculations results in the body and inertial frame accelerations
344 // Compute body and inertial frames accelerations based on the current body
345 // forces including centripetal and coriolis accelerations for the former.
346 // vOmegaEarth is the Earth angular rate - expressed in the inertial frame -
347 // so it has to be transformed to the body frame. More completely,
348 // vOmegaEarth is the rate of the ECEF frame relative to the Inertial
349 // frame (ECI), expressed in the Inertial frame.
350 // vForces is the total force on the vehicle in the body frame.
351 // VState.vPQR is the vehicle body rate relative to the ECEF frame, expressed
352 // in the body frame.
353 // VState.vUVW is the vehicle velocity relative to the ECEF frame, expressed
354 // in the body frame.
355 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
356 // Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
358 void FGPropagate::CalculateUVWdot(void)
360 double mass = FDMExec->GetMassBalance()->GetMass(); // mass
361 const FGColumnVector3& vForces = FDMExec->GetAircraft()->GetForces(); // current forces
363 vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
365 // Include Centripetal acceleration.
366 vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
368 // Include Gravitation accel
371 vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->GetGAccel(VehicleRadius) );
374 vGravAccel = Tec2b * FDMExec->GetInertial()->GetGravityJ2(VState.vLocation);
378 vUVWdot += vGravAccel;
379 vUVWidot = Tb2i * (vForces/mass + vGravAccel);
382 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
383 // Transform the velocity vector of the body relative to the origin (Earth
384 // center) to be expressed in the inertial frame, and add the vehicle velocity
385 // contribution due to the rotation of the planet.
386 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
387 // Second edition (2004), eqn 1.5-16c (page 50)
389 void FGPropagate::CalculateInertialVelocity(void)
391 VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
394 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
395 // Transform the velocity vector of the inertial frame to be expressed in the
396 // body frame relative to the origin (Earth center), and substract the vehicle
397 // velocity contribution due to the rotation of the planet.
399 void FGPropagate::CalculateUVW(void)
401 VState.vUVW = Ti2b * (VState.vInertialVelocity - (vOmegaEarth * VState.vInertialPosition));
404 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
406 void FGPropagate::Integrate( FGColumnVector3& Integrand,
407 FGColumnVector3& Val,
408 deque <FGColumnVector3>& ValDot,
410 eIntegrateType integration_type)
412 ValDot.push_front(Val);
415 switch(integration_type) {
416 case eRectEuler: Integrand += dt*ValDot[0];
418 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
420 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
422 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
424 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
426 case eNone: // do nothing, freeze translational rate
431 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
433 void FGPropagate::Integrate( FGQuaternion& Integrand,
435 deque <FGQuaternion>& ValDot,
437 eIntegrateType integration_type)
439 ValDot.push_front(Val);
442 switch(integration_type) {
443 case eRectEuler: Integrand += dt*ValDot[0];
445 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
447 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
449 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
451 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
453 case eNone: // do nothing, freeze rotational rate
458 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
459 // Evaluates the rates (translation or rotation) that the friction forces have
460 // to resist to. This includes the external forces and moments as well as the
461 // relative movement between the aircraft and the ground.
462 // Erin Catto's paper (see ref [6]) only supports Euler integration scheme and
463 // this algorithm has been adapted to handle the multistep algorithms that
464 // JSBSim supports (i.e. Trapezoidal, Adams-Bashforth 2, 3 and 4). The capacity
465 // to handle the multistep integration schemes adds some complexity but it
466 // significantly helps stabilizing the friction forces.
468 void FGPropagate::EvaluateRateToResistTo(FGColumnVector3& vdot,
469 const FGColumnVector3& Val,
470 const FGColumnVector3& ValDot,
471 const FGColumnVector3& LocalTerrainVal,
472 deque <FGColumnVector3>& dqValDot,
474 const eIntegrateType integration_type)
476 switch(integration_type) {
477 case eAdamsBashforth4:
478 vdot = ValDot + Ti2b * (-59.*dqValDot[0]+37.*dqValDot[1]-9.*dqValDot[2])/55.;
479 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
480 vdot += 24.*(Val - Tec2b * LocalTerrainVal) / (55.*dt);
482 case eAdamsBashforth3:
483 vdot = ValDot + Ti2b * (-16.*dqValDot[0]+5.*dqValDot[1])/23.;
484 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
485 vdot += 12.*(Val - Tec2b * LocalTerrainVal) / (23.*dt);
487 case eAdamsBashforth2:
488 vdot = ValDot - Ti2b * dqValDot[0]/3.;
489 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
490 vdot += 2.*(Val - Tec2b * LocalTerrainVal) / (3.*dt);
493 vdot = ValDot + Ti2b * dqValDot[0];
494 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
495 vdot += 2.*(Val - Tec2b * LocalTerrainVal) / dt;
499 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
500 vdot += (Val - Tec2b * LocalTerrainVal) / dt;
507 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
508 // Resolves the contact forces just before integrating the EOM.
509 // This routine is using Lagrange multipliers and the projected Gauss-Seidel
511 // Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence",
513 // In JSBSim there is only one rigid body (the aircraft) and there can be
514 // multiple points of contact between the aircraft and the ground. As a
515 // consequence our matrix J*M^-1*J^T is not sparse and the algorithm described
516 // in Catto's paper has been adapted accordingly.
517 // The friction forces are resolved in the body frame relative to the origin
520 void FGPropagate::ResolveFrictionForces(double dt)
522 const double invMass = 1.0 / FDMExec->GetMassBalance()->GetMass();
523 const FGMatrix33& Jinv = FDMExec->GetMassBalance()->GetJinv();
524 vector <FGColumnVector3> JacF, JacM;
525 vector<double> lambda, lambdaMin, lambdaMax;
526 FGColumnVector3 vdot, wdot;
527 FGColumnVector3 Fc, Mc;
530 // Compiles data from the ground reactions to build up the jacobian matrix
531 for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it, n++) {
532 JacF.push_back((*it)->ForceJacobian);
533 JacM.push_back((*it)->MomentJacobian);
534 lambda.push_back((*it)->value);
535 lambdaMax.push_back((*it)->Max);
536 lambdaMin.push_back((*it)->Min);
539 // If no gears are in contact with the ground then return
542 vector<double> a(n*n); // Will contain J*M^-1*J^T
543 vector<double> rhs(n);
545 // Assemble the linear system of equations
546 for (int i=0; i < n; i++) {
547 for (int j=0; j < i; j++)
548 a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of J^T*M^-1*J
549 for (int j=i; j < n; j++)
550 a[i*n+j] = DotProduct(JacF[i],invMass*JacF[j])+DotProduct(JacM[i],Jinv*JacM[j]);
553 // Assemble the RHS member
556 EvaluateRateToResistTo(vdot, VState.vUVW, vUVWdot, LocalTerrainVelocity,
557 VState.dqUVWidot, dt, integrator_translational_rate);
560 EvaluateRateToResistTo(wdot, VState.vPQR, vPQRdot, LocalTerrainAngularVelocity,
561 VState.dqPQRidot, dt, integrator_rotational_rate);
563 // Prepare the linear system for the Gauss-Seidel algorithm :
564 // 1. Compute the right hand side member 'rhs'
565 // 2. Divide every line of 'a' and 'lhs' by a[i,i]. This is in order to save
566 // a division computation at each iteration of Gauss-Seidel.
567 for (int i=0; i < n; i++) {
568 double d = 1.0 / a[i*n+i];
570 rhs[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
571 for (int j=0; j < n; j++)
575 // Resolve the Lagrange multipliers with the projected Gauss-Seidel method
576 for (int iter=0; iter < 50; iter++) {
579 for (int i=0; i < n; i++) {
580 double lambda0 = lambda[i];
581 double dlambda = rhs[i];
583 for (int j=0; j < n; j++)
584 dlambda -= a[i*n+j]*lambda[j];
586 lambda[i] = Constrain(lambdaMin[i], lambda0+dlambda, lambdaMax[i]);
587 dlambda = lambda[i] - lambda0;
589 norm += fabs(dlambda);
592 if (norm < 1E-5) break;
595 // Calculate the total friction forces and moments
600 for (int i=0; i< n; i++) {
601 Fc += lambda[i]*JacF[i];
602 Mc += lambda[i]*JacM[i];
605 vUVWdot += invMass * Fc;
606 vUVWidot += invMass * Tb2i * Fc;
607 vPQRdot += Jinv * Mc;
608 vPQRidot += Tb2i* Jinv * Mc;
610 // Save the value of the Lagrange multipliers to accelerate the convergence
611 // of the Gauss-Seidel algorithm at next iteration.
613 for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it)
614 (*it)->value = lambda[i++];
616 FDMExec->GetGroundReactions()->UpdateForcesAndMoments();
619 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
621 void FGPropagate::UpdateLocationMatrices(void)
623 Tl2ec = GetTl2ec(); // local to ECEF transform
624 Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
626 Tl2i = Ti2l.Transposed();
629 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
631 void FGPropagate::UpdateBodyMatrices(void)
633 Ti2b = GetTi2b(); // ECI to body frame transform
634 Tb2i = Ti2b.Transposed(); // body to ECI frame transform
635 Tl2b = Ti2b*Tl2i; // local to body frame transform
636 Tb2l = Tl2b.Transposed(); // body to local frame transform
637 Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
638 Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
641 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
643 void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
644 VState.qAttitudeECI = Qi;
645 VState.qAttitudeECI.Normalize();
646 UpdateBodyMatrices();
647 VState.qAttitudeLocal = Tl2b.GetQuaternion();
650 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
652 void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
653 VState.vInertialVelocity = Vi;
655 vVel = GetTb2l() * VState.vUVW;
658 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
660 void FGPropagate::SetInertialRates(FGColumnVector3 vRates) {
661 VState.vPQRi_i = vRates;
662 VState.vPQRi = Ti2b * VState.vPQRi_i;
663 VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
666 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
668 void FGPropagate::InitializeDerivatives(void)
670 // Make an initial run and set past values
671 CalculatePQRdot(); // Angular rate derivative
672 CalculateUVWdot(); // Translational rate derivative
673 ResolveFrictionForces(0.); // Update rate derivatives with friction forces
674 CalculateQuatdot(); // Angular orientation derivative
675 CalculateInertialVelocity(); // Translational position derivative
677 // Initialize past values deques
678 VState.dqPQRidot.clear();
679 VState.dqUVWidot.clear();
680 VState.dqInertialVelocity.clear();
681 VState.dqQtrndot.clear();
682 for (int i=0; i<4; i++) {
683 VState.dqPQRidot.push_front(vPQRidot);
684 VState.dqUVWidot.push_front(vUVWdot);
685 VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
686 VState.dqQtrndot.push_front(vQtrndot);
690 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
692 void FGPropagate::RecomputeLocalTerrainRadius(void)
694 FGLocation contactloc;
696 double t = FDMExec->GetSimTime();
698 // Get the LocalTerrain radius.
699 FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv,
700 LocalTerrainVelocity, LocalTerrainAngularVelocity);
701 LocalTerrainRadius = contactloc.GetRadius();
704 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
706 void FGPropagate::SetTerrainElevation(double terrainElev)
708 LocalTerrainRadius = terrainElev + SeaLevelRadius;
709 FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
712 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
714 double FGPropagate::GetTerrainElevation(void) const
716 return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
719 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
720 //Todo: when should this be called - when should the new EPA be used to
721 // calculate the transformation matrix, so that the matrix is not a step
722 // ahead of the sim and the associated calculations?
723 const FGMatrix33& FGPropagate::GetTi2ec(void)
725 return VState.vLocation.GetTi2ec();
728 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
730 const FGMatrix33& FGPropagate::GetTec2i(void)
732 return VState.vLocation.GetTec2i();
735 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
737 void FGPropagate::SetAltitudeASL(double altASL)
739 VState.vLocation.SetRadius( altASL + SeaLevelRadius );
742 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
744 double FGPropagate::GetLocalTerrainRadius(void) const
746 return LocalTerrainRadius;
749 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
751 double FGPropagate::GetDistanceAGL(void) const
753 return VState.vLocation.GetRadius() - LocalTerrainRadius;
756 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
758 void FGPropagate::SetDistanceAGL(double tt)
760 VState.vLocation.SetRadius( tt + LocalTerrainRadius );
763 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
765 void FGPropagate::DumpState(void)
769 << "------------------------------------------------------------------" << reset << endl;
771 << "State Report at sim time: " << FDMExec->GetSimTime() << " seconds" << reset << endl;
772 cout << " " << underon
773 << "Position" << underoff << endl;
774 cout << " ECI: " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl;
775 cout << " ECEF: " << VState.vLocation << " (x,y,z, in ft)" << endl;
776 cout << " Local: " << VState.vLocation.GetLatitudeDeg()
777 << ", " << VState.vLocation.GetLongitudeDeg()
778 << ", " << GetAltitudeASL() << " (lat, lon, alt in deg and ft)" << endl;
780 cout << endl << " " << underon
781 << "Orientation" << underoff << endl;
782 cout << " ECI: " << VState.qAttitudeECI.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
783 cout << " Local: " << VState.qAttitudeLocal.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
785 cout << endl << " " << underon
786 << "Velocity" << underoff << endl;
787 cout << " ECI: " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl;
788 cout << " ECEF: " << (GetTb2ec() * VState.vUVW).Dump(", ") << " (x,y,z in ft/s)" << endl;
789 cout << " Local: " << GetVel() << " (n,e,d in ft/sec)" << endl;
790 cout << " Body: " << GetUVW() << " (u,v,w in ft/sec)" << endl;
792 cout << endl << " " << underon
793 << "Body Rates (relative to given frame, expressed in body frame)" << underoff << endl;
794 cout << " ECI: " << (VState.vPQRi*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
795 cout << " ECEF: " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
798 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
800 void FGPropagate::bind(void)
802 typedef double (FGPropagate::*PMF)(int) const;
804 PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
806 PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
807 PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
808 PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
810 PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
811 PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
812 PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
814 PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
815 PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
816 PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
818 PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
819 PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
820 PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
822 PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
824 PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
825 PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
826 PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
828 PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
829 PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
830 PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
832 PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
833 PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
834 PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
835 PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
836 PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
837 PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
838 PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
839 PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
840 PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
841 PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
842 PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
843 PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
844 &FGPropagate::GetTerrainElevation,
845 &FGPropagate::SetTerrainElevation, false);
847 PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
849 PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
850 PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
851 PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
853 PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
854 PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
855 PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
857 PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
858 PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
859 PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
860 PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
861 PropertyManager->Tie("simulation/gravity-model", &gravType);
864 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
865 // The bitmasked value choices are as follows:
866 // unset: In this case (the default) JSBSim would only print
867 // out the normally expected messages, essentially echoing
868 // the config files as they are read. If the environment
869 // variable is not set, debug_lvl is set to 1 internally
870 // 0: This requests JSBSim not to output any messages
872 // 1: This value explicity requests the normal JSBSim
874 // 2: This value asks for a message to be printed out when
875 // a class is instantiated
876 // 4: When this value is set, a message is displayed when a
877 // FGModel object executes its Run() method
878 // 8: When this value is set, various runtime state variables
879 // are printed out periodically
880 // 16: When set various parameters are sanity checked and
881 // a message is printed out when they go out of bounds
883 void FGPropagate::Debug(int from)
885 if (debug_lvl <= 0) return;
887 if (debug_lvl & 1) { // Standard console startup message output
888 if (from == 0) { // Constructor
892 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
893 if (from == 0) cout << "Instantiated: FGPropagate" << endl;
894 if (from == 1) cout << "Destroyed: FGPropagate" << endl;
896 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
898 if (debug_lvl & 8 && from == 2) { // Runtime state variables
899 cout << endl << fgblue << highint << left
900 << " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
903 cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
904 << FDMExec->GetInertial()->GetEarthPositionAngleDeg() << endl;
906 cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
907 cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
908 cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
909 cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
910 cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
911 cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
912 cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
913 cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
915 cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
916 << reset << endl << Tec2b.Dump("\t", " ") << endl;
917 cout << highint << " Associated Euler angles (deg): " << setw(8)
918 << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
921 cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
922 << reset << endl << Tb2ec.Dump("\t", " ") << endl;
923 cout << highint << " Associated Euler angles (deg): " << setw(8)
924 << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
927 cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
928 << reset << endl << Tl2b.Dump("\t", " ") << endl;
929 cout << highint << " Associated Euler angles (deg): " << setw(8)
930 << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
933 cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
934 << reset << endl << Tb2l.Dump("\t", " ") << endl;
935 cout << highint << " Associated Euler angles (deg): " << setw(8)
936 << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
939 cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
940 << reset << endl << Tl2ec.Dump("\t", " ") << endl;
941 cout << highint << " Associated Euler angles (deg): " << setw(8)
942 << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
945 cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
946 << reset << endl << Tec2l.Dump("\t", " ") << endl;
947 cout << highint << " Associated Euler angles (deg): " << setw(8)
948 << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
951 cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
952 << reset << endl << Tec2i.Dump("\t", " ") << endl;
953 cout << highint << " Associated Euler angles (deg): " << setw(8)
954 << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
957 cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
958 << reset << endl << Ti2ec.Dump("\t", " ") << endl;
959 cout << highint << " Associated Euler angles (deg): " << setw(8)
960 << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
963 cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
964 << reset << endl << Ti2b.Dump("\t", " ") << endl;
965 cout << highint << " Associated Euler angles (deg): " << setw(8)
966 << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
969 cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
970 << reset << endl << Tb2i.Dump("\t", " ") << endl;
971 cout << highint << " Associated Euler angles (deg): " << setw(8)
972 << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
975 cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
976 << reset << endl << Ti2l.Dump("\t", " ") << endl;
977 cout << highint << " Associated Euler angles (deg): " << setw(8)
978 << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
981 cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
982 << reset << endl << Tl2i.Dump("\t", " ") << endl;
983 cout << highint << " Associated Euler angles (deg): " << setw(8)
984 << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
987 cout << setprecision(6); // reset the output stream
989 if (debug_lvl & 16) { // Sanity checking
990 if (from == 2) { // State sanity checking
991 if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
992 cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
995 if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
996 cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
999 if (fabs(GetDistanceAGL()) > 1e10) {
1000 cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
1005 if (debug_lvl & 64) {
1006 if (from == 0) { // Constructor
1007 cout << IdSrc << endl;
1008 cout << IdHdr << endl;