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)
88 vQtrndot = FGQuaternion(0,0,0);
90 vInertialVelocity.InitMatrix();
92 integrator_rotational_rate = eAdamsBashforth2;
93 integrator_translational_rate = eTrapezoidal;
94 integrator_rotational_position = eAdamsBashforth2;
95 integrator_translational_position = eTrapezoidal;
97 VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
98 VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
99 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
100 VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0));
106 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
108 FGPropagate::~FGPropagate(void)
113 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
115 bool FGPropagate::InitModel(void)
117 if (!FGModel::InitModel()) return false;
119 // For initialization ONLY:
120 SeaLevelRadius = LocalTerrainRadius = FDMExec->GetInertial()->GetRefRadius();
122 VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
123 VState.vLocation.SetEllipse(FDMExec->GetInertial()->GetSemimajor(), FDMExec->GetInertial()->GetSemiminor());
124 vOmegaEarth = FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->omega() ); // Earth rotation vector
126 vPQRdot.InitMatrix();
127 vQtrndot = FGQuaternion(0,0,0);
128 vUVWdot.InitMatrix();
129 vInertialVelocity.InitMatrix();
131 VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
132 VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
133 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
134 VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
136 integrator_rotational_rate = eAdamsBashforth2;
137 integrator_translational_rate = eTrapezoidal;
138 integrator_rotational_position = eAdamsBashforth2;
139 integrator_translational_position = eTrapezoidal;
144 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
146 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
148 SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
149 SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
151 // Initialize the State Vector elements and the transformation matrices
153 // Set the position lat/lon/radius
154 VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
155 FGIC->GetLatitudeRadIC(),
156 FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
158 VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
160 Ti2ec = GetTi2ec(); // ECI to ECEF transform
161 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
163 VState.vInertialPosition = Tec2i * VState.vLocation;
165 UpdateLocationMatrices();
167 // Set the orientation from the euler angles (is normalized within the
168 // constructor). The Euler angles represent the orientation of the body
169 // frame relative to the local frame.
170 VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(),
171 FGIC->GetThetaRadIC(),
172 FGIC->GetPsiRadIC() );
174 VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
175 UpdateBodyMatrices();
177 // Set the velocities in the instantaneus body frame
178 VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
179 FGIC->GetVBodyFpsIC(),
180 FGIC->GetWBodyFpsIC() );
182 // Compute the local frame ECEF velocity
183 vVel = Tb2l * VState.vUVW;
185 // Recompute the LocalTerrainRadius.
186 RecomputeLocalTerrainRadius();
188 VehicleRadius = GetRadius();
189 double radInv = 1.0/VehicleRadius;
191 // Refer to Stevens and Lewis, 1.5-14a, pg. 49.
192 // This is the rotation rate of the "Local" frame, expressed in the local frame.
194 FGColumnVector3 vOmegaLocal = FGColumnVector3(
196 -radInv*vVel(eNorth),
197 -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
199 // Set the angular velocities of the body frame relative to the ECEF frame,
200 // expressed in the body frame. Effectively, this is:
201 // w_b/e = w_b/l + w_l/e
202 VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
204 FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal;
206 VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
207 VState.vPQRi_i = Tb2i * VState.vPQRi;
209 // Make an initial run and set past values
210 InitializeDerivatives();
213 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
215 Purpose: Called on a schedule to perform EOM integration
216 Notes: [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
217 In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
219 At the top of this Run() function, see several "shortcuts" (or, aliases) being
220 set up for use later, rather than using the longer class->function() notation.
222 This propagation is done using the current state values
223 and current derivatives. Based on these values we compute an approximation to the
224 state values for (now + dt).
226 In the code below, variables named beginning with a small "v" refer to a
227 a column vector, variables named beginning with a "T" refer to a transformation
228 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
233 bool FGPropagate::Run(void)
235 if (FGModel::Run()) return true; // Fast return if we have nothing to do ...
236 if (FDMExec->Holding()) return false;
238 double dt = FDMExec->GetDeltaT()*rate; // The 'stepsize'
242 // Calculate state derivatives
243 CalculatePQRdot(); // Angular rate derivative
244 CalculateUVWdot(); // Translational rate derivative
245 ResolveFrictionForces(dt); // Update rate derivatives with friction forces
246 CalculateQuatdot(); // Angular orientation derivative
247 CalculateUVW(); // Translational position derivative (velocities are integrated in the inertial frame)
249 // Propagate rotational / translational velocity, angular /translational position, respectively.
251 Integrate(VState.vPQRi_i, vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate); // ECI integration
252 Integrate(VState.qAttitudeECI, vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
253 Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
254 Integrate(VState.vInertialVelocity, vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate);
256 // CAUTION : the order of the operations below is very important to get transformation
257 // matrices that are consistent with the new state of the vehicle
259 // 1. Update the Earth position angle (EPA)
260 VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
262 // 2. Update the Ti2ec and Tec2i transforms from the updated EPA
263 Ti2ec = GetTi2ec(); // ECI to ECEF transform
264 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
266 // 3. Update the location from the updated Ti2ec and inertial position
267 VState.vLocation = Ti2ec*VState.vInertialPosition;
269 // 4. Update the other "Location-based" transformation matrices from the updated
271 UpdateLocationMatrices();
273 // 5. Normalize the ECI Attitude quaternion
274 VState.qAttitudeECI.Normalize();
276 // 6. Update the "Orientation-based" transformation matrices from the updated
277 // orientation quaternion and vLocation vector.
278 UpdateBodyMatrices();
280 // Set auxililary state variables
281 RecomputeLocalTerrainRadius();
283 VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
285 VState.vPQRi = Ti2b * VState.vPQRi_i;
286 VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
288 VState.qAttitudeLocal = Tl2b.GetQuaternion();
290 // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
291 vVel = Tb2l * VState.vUVW;
299 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
300 // Compute body frame rotational accelerations based on the current body moments
302 // vPQRdot is the derivative of the absolute angular velocity of the vehicle
303 // (body rate with respect to the inertial frame), expressed in the body frame,
304 // where the derivative is taken in the body frame.
305 // J is the inertia matrix
306 // Jinv is the inverse inertia matrix
307 // vMoments is the moment vector in the body frame
308 // VState.vPQRi is the total inertial angular velocity of the vehicle
309 // expressed in the body frame.
310 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
311 // Second edition (2004), eqn 1.5-16e (page 50)
313 void FGPropagate::CalculatePQRdot(void)
315 const FGColumnVector3& vMoments = FDMExec->GetAircraft()->GetMoments(); // current moments
316 const FGMatrix33& J = FDMExec->GetMassBalance()->GetJ(); // inertia matrix
317 const FGMatrix33& Jinv = FDMExec->GetMassBalance()->GetJinv(); // inertia matrix inverse
319 // Compute body frame rotational accelerations based on the current body
320 // moments and the total inertial angular velocity expressed in the body
323 vPQRdot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
324 vPQRidot = Tb2i * vPQRdot;
327 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
328 // Compute the quaternion orientation derivative
330 // vQtrndot is the quaternion derivative.
331 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
332 // Second edition (2004), eqn 1.5-16b (page 50)
334 void FGPropagate::CalculateQuatdot(void)
336 // Compute quaternion orientation derivative on current body rates
337 vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi);
340 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
341 // This set of calculations results in the body and inertial frame accelerations
343 // Compute body and inertial frames accelerations based on the current body
344 // forces including centripetal and coriolis accelerations for the former.
345 // vOmegaEarth is the Earth angular rate - expressed in the inertial frame -
346 // so it has to be transformed to the body frame. More completely,
347 // vOmegaEarth is the rate of the ECEF frame relative to the Inertial
348 // frame (ECI), expressed in the Inertial frame.
349 // vForces is the total force on the vehicle in the body frame.
350 // VState.vPQR is the vehicle body rate relative to the ECEF frame, expressed
351 // in the body frame.
352 // VState.vUVW is the vehicle velocity relative to the ECEF frame, expressed
353 // in the body frame.
354 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
355 // Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
357 void FGPropagate::CalculateUVWdot(void)
359 double mass = FDMExec->GetMassBalance()->GetMass(); // mass
360 const FGColumnVector3& vForces = FDMExec->GetAircraft()->GetForces(); // current forces
362 vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
364 // Include Centripetal acceleration.
365 vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
367 // Include Gravitation accel
370 vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, FDMExec->GetInertial()->GetGAccel(VehicleRadius) );
373 vGravAccel = Tec2b * FDMExec->GetInertial()->GetGravityJ2(VState.vLocation);
377 vUVWdot += vGravAccel;
378 vUVWidot = Tb2i * (vForces/mass + vGravAccel);
381 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
382 // Transform the velocity vector of the body relative to the origin (Earth
383 // center) to be expressed in the inertial frame, and add the vehicle velocity
384 // contribution due to the rotation of the planet.
385 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
386 // Second edition (2004), eqn 1.5-16c (page 50)
388 void FGPropagate::CalculateInertialVelocity(void)
390 VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
393 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
394 // Transform the velocity vector of the inertial frame to be expressed in the
395 // body frame relative to the origin (Earth center), and substract the vehicle
396 // velocity contribution due to the rotation of the planet.
398 void FGPropagate::CalculateUVW(void)
400 VState.vUVW = Ti2b * (VState.vInertialVelocity - (vOmegaEarth * VState.vInertialPosition));
403 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
405 void FGPropagate::Integrate( FGColumnVector3& Integrand,
406 FGColumnVector3& Val,
407 deque <FGColumnVector3>& ValDot,
409 eIntegrateType integration_type)
411 ValDot.push_front(Val);
414 switch(integration_type) {
415 case eRectEuler: Integrand += dt*ValDot[0];
417 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
419 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
421 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
423 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
425 case eNone: // do nothing, freeze translational rate
430 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
432 void FGPropagate::Integrate( FGQuaternion& Integrand,
434 deque <FGQuaternion>& ValDot,
436 eIntegrateType integration_type)
438 ValDot.push_front(Val);
441 switch(integration_type) {
442 case eRectEuler: Integrand += dt*ValDot[0];
444 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
446 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
448 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
450 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
452 case eNone: // do nothing, freeze rotational rate
457 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
458 // Evaluates the rates (translation or rotation) that the friction forces have
459 // to resist to. This includes the external forces and moments as well as the
460 // relative movement between the aircraft and the ground.
461 // Erin Catto's paper (see ref [6]) only supports Euler integration scheme and
462 // this algorithm has been adapted to handle the multistep algorithms that
463 // JSBSim supports (i.e. Trapezoidal, Adams-Bashforth 2, 3 and 4). The capacity
464 // to handle the multistep integration schemes adds some complexity but it
465 // significantly helps stabilizing the friction forces.
467 void FGPropagate::EvaluateRateToResistTo(FGColumnVector3& vdot,
468 const FGColumnVector3& Val,
469 const FGColumnVector3& ValDot,
470 const FGColumnVector3& LocalTerrainVal,
471 deque <FGColumnVector3>& dqValDot,
473 const eIntegrateType integration_type)
475 switch(integration_type) {
476 case eAdamsBashforth4:
477 vdot = ValDot + Ti2b * (-59.*dqValDot[0]+37.*dqValDot[1]-9.*dqValDot[2])/55.;
478 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
479 vdot += 24.*(Val - Tec2b * LocalTerrainVal) / (55.*dt);
481 case eAdamsBashforth3:
482 vdot = ValDot + Ti2b * (-16.*dqValDot[0]+5.*dqValDot[1])/23.;
483 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
484 vdot += 12.*(Val - Tec2b * LocalTerrainVal) / (23.*dt);
486 case eAdamsBashforth2:
487 vdot = ValDot - Ti2b * dqValDot[0]/3.;
488 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
489 vdot += 2.*(Val - Tec2b * LocalTerrainVal) / (3.*dt);
492 vdot = ValDot + Ti2b * dqValDot[0];
493 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
494 vdot += 2.*(Val - Tec2b * LocalTerrainVal) / dt;
498 if (dt > 0.) // Zeroes out the relative movement between aircraft and ground
499 vdot += (Val - Tec2b * LocalTerrainVal) / dt;
506 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
507 // Resolves the contact forces just before integrating the EOM.
508 // This routine is using Lagrange multipliers and the projected Gauss-Seidel
510 // Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence",
512 // In JSBSim there is only one rigid body (the aircraft) and there can be
513 // multiple points of contact between the aircraft and the ground. As a
514 // consequence our matrix J*M^-1*J^T is not sparse and the algorithm described
515 // in Catto's paper has been adapted accordingly.
516 // The friction forces are resolved in the body frame relative to the origin
519 void FGPropagate::ResolveFrictionForces(double dt)
521 const double invMass = 1.0 / FDMExec->GetMassBalance()->GetMass();
522 const FGMatrix33& Jinv = FDMExec->GetMassBalance()->GetJinv();
523 vector <FGColumnVector3> JacF, JacM;
524 vector<double> lambda, lambdaMin, lambdaMax;
525 FGColumnVector3 vdot, wdot;
526 FGColumnVector3 Fc, Mc;
529 // Compiles data from the ground reactions to build up the jacobian matrix
530 for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it, n++) {
531 JacF.push_back((*it)->ForceJacobian);
532 JacM.push_back((*it)->MomentJacobian);
533 lambda.push_back((*it)->value);
534 lambdaMax.push_back((*it)->Max);
535 lambdaMin.push_back((*it)->Min);
538 // If no gears are in contact with the ground then return
541 vector<double> a(n*n); // Will contain J*M^-1*J^T
542 vector<double> rhs(n);
544 // Assemble the linear system of equations
545 for (int i=0; i < n; i++) {
546 for (int j=0; j < i; j++)
547 a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of J^T*M^-1*J
548 for (int j=i; j < n; j++)
549 a[i*n+j] = DotProduct(JacF[i],invMass*JacF[j])+DotProduct(JacM[i],Jinv*JacM[j]);
552 // Assemble the RHS member
555 EvaluateRateToResistTo(vdot, VState.vUVW, vUVWdot, LocalTerrainVelocity,
556 VState.dqUVWidot, dt, integrator_translational_rate);
559 EvaluateRateToResistTo(wdot, VState.vPQR, vPQRdot, LocalTerrainAngularVelocity,
560 VState.dqPQRidot, dt, integrator_rotational_rate);
562 // Prepare the linear system for the Gauss-Seidel algorithm :
563 // 1. Compute the right hand side member 'rhs'
564 // 2. Divide every line of 'a' and 'lhs' by a[i,i]. This is in order to save
565 // a division computation at each iteration of Gauss-Seidel.
566 for (int i=0; i < n; i++) {
567 double d = 1.0 / a[i*n+i];
569 rhs[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
570 for (int j=0; j < n; j++)
574 // Resolve the Lagrange multipliers with the projected Gauss-Seidel method
575 for (int iter=0; iter < 50; iter++) {
578 for (int i=0; i < n; i++) {
579 double lambda0 = lambda[i];
580 double dlambda = rhs[i];
582 for (int j=0; j < n; j++)
583 dlambda -= a[i*n+j]*lambda[j];
585 lambda[i] = Constrain(lambdaMin[i], lambda0+dlambda, lambdaMax[i]);
586 dlambda = lambda[i] - lambda0;
588 norm += fabs(dlambda);
591 if (norm < 1E-5) break;
594 // Calculate the total friction forces and moments
599 for (int i=0; i< n; i++) {
600 Fc += lambda[i]*JacF[i];
601 Mc += lambda[i]*JacM[i];
604 vUVWdot += invMass * Fc;
605 vUVWidot += invMass * Tb2i * Fc;
606 vPQRdot += Jinv * Mc;
607 vPQRidot += Tb2i* Jinv * Mc;
609 // Save the value of the Lagrange multipliers to accelerate the convergence
610 // of the Gauss-Seidel algorithm at next iteration.
612 for (MultiplierIterator it=MultiplierIterator(FDMExec->GetGroundReactions()); *it; ++it)
613 (*it)->value = lambda[i++];
615 FDMExec->GetGroundReactions()->UpdateForcesAndMoments();
618 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
620 void FGPropagate::UpdateLocationMatrices(void)
622 Tl2ec = GetTl2ec(); // local to ECEF transform
623 Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
625 Tl2i = Ti2l.Transposed();
628 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
630 void FGPropagate::UpdateBodyMatrices(void)
632 Ti2b = GetTi2b(); // ECI to body frame transform
633 Tb2i = Ti2b.Transposed(); // body to ECI frame transform
634 Tl2b = Ti2b*Tl2i; // local to body frame transform
635 Tb2l = Tl2b.Transposed(); // body to local frame transform
636 Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
637 Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
640 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
642 void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
643 VState.qAttitudeECI = Qi;
644 VState.qAttitudeECI.Normalize();
645 UpdateBodyMatrices();
646 VState.qAttitudeLocal = Tl2b.GetQuaternion();
649 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
651 void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
652 VState.vInertialVelocity = Vi;
654 vVel = GetTb2l() * VState.vUVW;
657 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
659 void FGPropagate::SetInertialRates(FGColumnVector3 vRates) {
660 VState.vPQRi_i = vRates;
661 VState.vPQRi = Ti2b * VState.vPQRi_i;
662 VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
665 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
667 void FGPropagate::InitializeDerivatives(void)
669 // Make an initial run and set past values
670 CalculatePQRdot(); // Angular rate derivative
671 CalculateUVWdot(); // Translational rate derivative
672 ResolveFrictionForces(0.); // Update rate derivatives with friction forces
673 CalculateQuatdot(); // Angular orientation derivative
674 CalculateInertialVelocity(); // Translational position derivative
676 // Initialize past values deques
677 VState.dqPQRidot.clear();
678 VState.dqUVWidot.clear();
679 VState.dqInertialVelocity.clear();
680 VState.dqQtrndot.clear();
681 for (int i=0; i<4; i++) {
682 VState.dqPQRidot.push_front(vPQRidot);
683 VState.dqUVWidot.push_front(vUVWdot);
684 VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
685 VState.dqQtrndot.push_front(vQtrndot);
689 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
691 void FGPropagate::RecomputeLocalTerrainRadius(void)
693 FGLocation contactloc;
695 double t = FDMExec->GetSimTime();
697 // Get the LocalTerrain radius.
698 FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv,
699 LocalTerrainVelocity, LocalTerrainAngularVelocity);
700 LocalTerrainRadius = contactloc.GetRadius();
703 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
705 void FGPropagate::SetTerrainElevation(double terrainElev)
707 LocalTerrainRadius = terrainElev + SeaLevelRadius;
708 FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
711 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
713 double FGPropagate::GetTerrainElevation(void) const
715 return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
718 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
719 //Todo: when should this be called - when should the new EPA be used to
720 // calculate the transformation matrix, so that the matrix is not a step
721 // ahead of the sim and the associated calculations?
722 const FGMatrix33& FGPropagate::GetTi2ec(void)
724 return VState.vLocation.GetTi2ec();
727 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
729 const FGMatrix33& FGPropagate::GetTec2i(void)
731 return VState.vLocation.GetTec2i();
734 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
736 void FGPropagate::SetAltitudeASL(double altASL)
738 VState.vLocation.SetRadius( altASL + SeaLevelRadius );
741 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
743 double FGPropagate::GetLocalTerrainRadius(void) const
745 return LocalTerrainRadius;
748 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
750 double FGPropagate::GetDistanceAGL(void) const
752 return VState.vLocation.GetRadius() - LocalTerrainRadius;
755 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
757 void FGPropagate::SetDistanceAGL(double tt)
759 VState.vLocation.SetRadius( tt + LocalTerrainRadius );
762 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
764 void FGPropagate::DumpState(void)
768 << "------------------------------------------------------------------" << reset << endl;
770 << "State Report at sim time: " << FDMExec->GetSimTime() << " seconds" << reset << endl;
771 cout << " " << underon
772 << "Position" << underoff << endl;
773 cout << " ECI: " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl;
774 cout << " ECEF: " << VState.vLocation << " (x,y,z, in ft)" << endl;
775 cout << " Local: " << VState.vLocation.GetLatitudeDeg()
776 << ", " << VState.vLocation.GetLongitudeDeg()
777 << ", " << GetAltitudeASL() << " (lat, lon, alt in deg and ft)" << endl;
779 cout << endl << " " << underon
780 << "Orientation" << underoff << endl;
781 cout << " ECI: " << VState.qAttitudeECI.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
782 cout << " Local: " << VState.qAttitudeLocal.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
784 cout << endl << " " << underon
785 << "Velocity" << underoff << endl;
786 cout << " ECI: " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl;
787 cout << " ECEF: " << (GetTb2ec() * VState.vUVW).Dump(", ") << " (x,y,z in ft/s)" << endl;
788 cout << " Local: " << GetVel() << " (n,e,d in ft/sec)" << endl;
789 cout << " Body: " << GetUVW() << " (u,v,w in ft/sec)" << endl;
791 cout << endl << " " << underon
792 << "Body Rates (relative to given frame, expressed in body frame)" << underoff << endl;
793 cout << " ECI: " << (VState.vPQRi*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
794 cout << " ECEF: " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
797 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
799 void FGPropagate::bind(void)
801 typedef double (FGPropagate::*PMF)(int) const;
803 PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
805 PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
806 PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
807 PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
809 PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
810 PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
811 PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
813 PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
814 PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
815 PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
817 PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
818 PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
819 PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
821 PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
823 PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
824 PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
825 PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
827 PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
828 PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
829 PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
831 PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
832 PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
833 PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
834 PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
835 PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
836 PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
837 PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
838 PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
839 PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
840 PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
841 PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
842 PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
843 &FGPropagate::GetTerrainElevation,
844 &FGPropagate::SetTerrainElevation, false);
846 PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
848 PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
849 PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
850 PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
852 PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
853 PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
854 PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
856 PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
857 PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
858 PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
859 PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
860 PropertyManager->Tie("simulation/gravity-model", &gravType);
863 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
864 // The bitmasked value choices are as follows:
865 // unset: In this case (the default) JSBSim would only print
866 // out the normally expected messages, essentially echoing
867 // the config files as they are read. If the environment
868 // variable is not set, debug_lvl is set to 1 internally
869 // 0: This requests JSBSim not to output any messages
871 // 1: This value explicity requests the normal JSBSim
873 // 2: This value asks for a message to be printed out when
874 // a class is instantiated
875 // 4: When this value is set, a message is displayed when a
876 // FGModel object executes its Run() method
877 // 8: When this value is set, various runtime state variables
878 // are printed out periodically
879 // 16: When set various parameters are sanity checked and
880 // a message is printed out when they go out of bounds
882 void FGPropagate::Debug(int from)
884 if (debug_lvl <= 0) return;
886 if (debug_lvl & 1) { // Standard console startup message output
887 if (from == 0) { // Constructor
891 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
892 if (from == 0) cout << "Instantiated: FGPropagate" << endl;
893 if (from == 1) cout << "Destroyed: FGPropagate" << endl;
895 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
897 if (debug_lvl & 8 && from == 2) { // Runtime state variables
898 cout << endl << fgblue << highint << left
899 << " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
902 cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
903 << FDMExec->GetInertial()->GetEarthPositionAngleDeg() << endl;
905 cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
906 cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
907 cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
908 cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
909 cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
910 cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
911 cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
912 cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
914 cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
915 << reset << endl << Tec2b.Dump("\t", " ") << endl;
916 cout << highint << " Associated Euler angles (deg): " << setw(8)
917 << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
920 cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
921 << reset << endl << Tb2ec.Dump("\t", " ") << endl;
922 cout << highint << " Associated Euler angles (deg): " << setw(8)
923 << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
926 cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
927 << reset << endl << Tl2b.Dump("\t", " ") << endl;
928 cout << highint << " Associated Euler angles (deg): " << setw(8)
929 << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
932 cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
933 << reset << endl << Tb2l.Dump("\t", " ") << endl;
934 cout << highint << " Associated Euler angles (deg): " << setw(8)
935 << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
938 cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
939 << reset << endl << Tl2ec.Dump("\t", " ") << endl;
940 cout << highint << " Associated Euler angles (deg): " << setw(8)
941 << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
944 cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
945 << reset << endl << Tec2l.Dump("\t", " ") << endl;
946 cout << highint << " Associated Euler angles (deg): " << setw(8)
947 << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
950 cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
951 << reset << endl << Tec2i.Dump("\t", " ") << endl;
952 cout << highint << " Associated Euler angles (deg): " << setw(8)
953 << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
956 cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
957 << reset << endl << Ti2ec.Dump("\t", " ") << endl;
958 cout << highint << " Associated Euler angles (deg): " << setw(8)
959 << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
962 cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
963 << reset << endl << Ti2b.Dump("\t", " ") << endl;
964 cout << highint << " Associated Euler angles (deg): " << setw(8)
965 << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
968 cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
969 << reset << endl << Tb2i.Dump("\t", " ") << endl;
970 cout << highint << " Associated Euler angles (deg): " << setw(8)
971 << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
974 cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
975 << reset << endl << Ti2l.Dump("\t", " ") << endl;
976 cout << highint << " Associated Euler angles (deg): " << setw(8)
977 << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
980 cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
981 << reset << endl << Tl2i.Dump("\t", " ") << endl;
982 cout << highint << " Associated Euler angles (deg): " << setw(8)
983 << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
986 cout << setprecision(6); // reset the output stream
988 if (debug_lvl & 16) { // Sanity checking
989 if (from == 2) { // State sanity checking
990 if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
991 cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
994 if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
995 cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
998 if (fabs(GetDistanceAGL()) > 1e10) {
999 cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
1004 if (debug_lvl & 64) {
1005 if (from == 0) { // Constructor
1006 cout << IdSrc << endl;
1007 cout << IdHdr << endl;