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.71 2010/10/15 11:34:09 jberndt 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.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
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 = Inertial->GetRefRadius();
123 VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
124 VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
125 vOmegaEarth = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
127 vPQRdot.InitMatrix();
128 vQtrndot = FGQuaternion(0,0,0);
129 vUVWdot.InitMatrix();
130 vInertialVelocity.InitMatrix();
132 VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
133 VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
134 VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
135 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
136 VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
138 integrator_rotational_rate = eAdamsBashforth2;
139 integrator_translational_rate = eTrapezoidal;
140 integrator_rotational_position = eAdamsBashforth2;
141 integrator_translational_position = eTrapezoidal;
146 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
148 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
150 SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
151 SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
153 // Initialize the State Vector elements and the transformation matrices
155 // Set the position lat/lon/radius
156 VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
157 FGIC->GetLatitudeRadIC(),
158 FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
160 VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle());
162 Ti2ec = GetTi2ec(); // ECI to ECEF transform
163 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
165 VState.vInertialPosition = Tec2i * VState.vLocation;
167 UpdateLocationMatrices();
169 // Set the orientation from the euler angles (is normalized within the
170 // constructor). The Euler angles represent the orientation of the body
171 // frame relative to the local frame.
172 VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(),
173 FGIC->GetThetaRadIC(),
174 FGIC->GetPsiRadIC() );
176 VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
177 UpdateBodyMatrices();
179 // Set the velocities in the instantaneus body frame
180 VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
181 FGIC->GetVBodyFpsIC(),
182 FGIC->GetWBodyFpsIC() );
184 // Compute the local frame ECEF velocity
185 vVel = Tb2l * VState.vUVW;
187 // Recompute the LocalTerrainRadius.
188 RecomputeLocalTerrainRadius();
190 VehicleRadius = GetRadius();
191 double radInv = 1.0/VehicleRadius;
193 // Refer to Stevens and Lewis, 1.5-14a, pg. 49.
194 // This is the rotation rate of the "Local" frame, expressed in the local frame.
196 FGColumnVector3 vOmegaLocal = FGColumnVector3(
198 -radInv*vVel(eNorth),
199 -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
201 // Set the angular velocities of the body frame relative to the ECEF frame,
202 // expressed in the body frame. Effectively, this is:
203 // w_b/e = w_b/l + w_l/e
204 VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
206 FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal;
208 VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
209 VState.vPQRi_i = Tb2i * VState.vPQRi;
211 // Make an initial run and set past values
212 InitializeDerivatives();
215 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
217 Purpose: Called on a schedule to perform EOM integration
218 Notes: [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
219 In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
221 At the top of this Run() function, see several "shortcuts" (or, aliases) being
222 set up for use later, rather than using the longer class->function() notation.
224 This propagation is done using the current state values
225 and current derivatives. Based on these values we compute an approximation to the
226 state values for (now + dt).
228 In the code below, variables named beginning with a small "v" refer to a
229 a column vector, variables named beginning with a "T" refer to a transformation
230 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
235 bool FGPropagate::Run(void)
237 if (FGModel::Run()) return true; // Fast return if we have nothing to do ...
238 if (FDMExec->Holding()) return false;
240 double dt = FDMExec->GetDeltaT()*rate; // The 'stepsize'
244 // Calculate state derivatives
245 CalculatePQRdot(); // Angular rate derivative
246 CalculateUVWdot(); // Translational rate derivative
247 ResolveFrictionForces(dt); // Update rate derivatives with friction forces
248 CalculateQuatdot(); // Angular orientation derivative
249 CalculateUVW(); // Translational position derivative (velocities are integrated in the inertial frame)
251 // Propagate rotational / translational velocity, angular /translational position, respectively.
253 Integrate(VState.vPQRi_i, vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate); // ECI integration
254 Integrate(VState.qAttitudeECI, vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
255 Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
256 Integrate(VState.vInertialVelocity, vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate);
258 // CAUTION : the order of the operations below is very important to get transformation
259 // matrices that are consistent with the new state of the vehicle
261 // 1. Update the Earth position angle (EPA)
262 VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle());
264 // 2. Update the Ti2ec and Tec2i transforms from the updated EPA
265 Ti2ec = GetTi2ec(); // ECI to ECEF transform
266 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
268 // 3. Update the location from the updated Ti2ec and inertial position
269 VState.vLocation = Ti2ec*VState.vInertialPosition;
271 // 4. Update the other "Location-based" transformation matrices from the updated
273 UpdateLocationMatrices();
275 // 5. Normalize the ECI Attitude quaternion
276 VState.qAttitudeECI.Normalize();
278 // 6. Update the "Orientation-based" transformation matrices from the updated
279 // orientation quaternion and vLocation vector.
280 UpdateBodyMatrices();
282 // Set auxililary state variables
283 RecomputeLocalTerrainRadius();
285 VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
287 VState.vPQRi = Ti2b * VState.vPQRi_i;
288 VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
290 VState.qAttitudeLocal = Tl2b.GetQuaternion();
292 // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
293 vVel = Tb2l * VState.vUVW;
301 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
302 // Compute body frame rotational accelerations based on the current body moments
304 // vPQRdot is the derivative of the absolute angular velocity of the vehicle
305 // (body rate with respect to the inertial frame), expressed in the body frame,
306 // where the derivative is taken in the body frame.
307 // J is the inertia matrix
308 // Jinv is the inverse inertia matrix
309 // vMoments is the moment vector in the body frame
310 // VState.vPQRi is the total inertial angular velocity of the vehicle
311 // expressed in the body frame.
312 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
313 // Second edition (2004), eqn 1.5-16e (page 50)
315 void FGPropagate::CalculatePQRdot(void)
317 const FGColumnVector3& vMoments = Aircraft->GetMoments(); // current moments
318 const FGMatrix33& J = MassBalance->GetJ(); // inertia matrix
319 const FGMatrix33& Jinv = MassBalance->GetJinv(); // inertia matrix inverse
321 // Compute body frame rotational accelerations based on the current body
322 // moments and the total inertial angular velocity expressed in the body
325 vPQRdot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
326 vPQRidot = Tb2i * vPQRdot;
329 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
330 // Compute the quaternion orientation derivative
332 // vQtrndot is the quaternion derivative.
333 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
334 // Second edition (2004), eqn 1.5-16b (page 50)
336 void FGPropagate::CalculateQuatdot(void)
338 // Compute quaternion orientation derivative on current body rates
339 vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi);
342 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
343 // This set of calculations results in the body and inertial frame accelerations
345 // Compute body and inertial frames accelerations based on the current body
346 // forces including centripetal and coriolis accelerations for the former.
347 // vOmegaEarth is the Earth angular rate - expressed in the inertial frame -
348 // so it has to be transformed to the body frame. More completely,
349 // vOmegaEarth is the rate of the ECEF frame relative to the Inertial
350 // frame (ECI), expressed in the Inertial frame.
351 // vForces is the total force on the vehicle in the body frame.
352 // VState.vPQR is the vehicle body rate relative to the ECEF frame, expressed
353 // in the body frame.
354 // VState.vUVW is the vehicle velocity relative to the ECEF frame, expressed
355 // in the body frame.
356 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
357 // Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
359 void FGPropagate::CalculateUVWdot(void)
361 double mass = MassBalance->GetMass(); // mass
362 const FGColumnVector3& vForces = Aircraft->GetForces(); // current forces
364 vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
366 // Include Centripetal acceleration.
367 vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
369 // Include Gravitation accel
372 vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) );
375 vGravAccel = Tec2b * Inertial->GetGravityJ2(VState.vLocation);
379 vUVWdot += vGravAccel;
380 vUVWidot = Tb2i * (vForces/mass + vGravAccel);
383 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
384 // Transform the velocity vector of the body relative to the origin (Earth
385 // center) to be expressed in the inertial frame, and add the vehicle velocity
386 // contribution due to the rotation of the planet.
387 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
388 // Second edition (2004), eqn 1.5-16c (page 50)
390 void FGPropagate::CalculateInertialVelocity(void)
392 VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
395 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
396 // Transform the velocity vector of the inertial frame to be expressed in the
397 // body frame relative to the origin (Earth center), and substract the vehicle
398 // velocity contribution due to the rotation of the planet.
400 void FGPropagate::CalculateUVW(void)
402 VState.vUVW = Ti2b * (VState.vInertialVelocity - (vOmegaEarth * VState.vInertialPosition));
405 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
407 void FGPropagate::Integrate( FGColumnVector3& Integrand,
408 FGColumnVector3& Val,
409 deque <FGColumnVector3>& ValDot,
411 eIntegrateType integration_type)
413 ValDot.push_front(Val);
416 switch(integration_type) {
417 case eRectEuler: Integrand += dt*ValDot[0];
419 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
421 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
423 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
425 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
427 case eNone: // do nothing, freeze translational rate
432 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
434 void FGPropagate::Integrate( FGQuaternion& Integrand,
436 deque <FGQuaternion>& ValDot,
438 eIntegrateType integration_type)
440 ValDot.push_front(Val);
443 switch(integration_type) {
444 case eRectEuler: Integrand += dt*ValDot[0];
446 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
448 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
450 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
452 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
454 case eNone: // do nothing, freeze rotational rate
459 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
460 // Resolves the contact forces just before integrating the EOM.
461 // This routine is using Lagrange multipliers and the projected Gauss-Seidel
463 // Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence",
465 // In JSBSim there is only one rigid body (the aircraft) and there can be
466 // multiple points of contact between the aircraft and the ground. As a
467 // consequence our matrix J*M^-1*J^T is not sparse and the algorithm described
468 // in Catto's paper has been adapted accordingly.
469 // The friction forces are resolved in the body frame relative to the origin
472 void FGPropagate::ResolveFrictionForces(double dt)
474 const double invMass = 1.0 / MassBalance->GetMass();
475 const FGMatrix33& Jinv = MassBalance->GetJinv();
476 vector <FGColumnVector3> JacF, JacM;
477 FGColumnVector3 vdot, wdot;
478 FGColumnVector3 Fc, Mc;
481 // Compiles data from the ground reactions to build up the jacobian matrix
482 for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, n++) {
483 JacF.push_back((*it)->ForceJacobian);
484 JacM.push_back((*it)->MomentJacobian);
487 // If no gears are in contact with the ground then return
490 vector<double> a(n*n); // Will contain J*M^-1*J^T
491 vector<double> eta(n);
492 vector<double> lambda(n);
493 vector<double> lambdaMin(n);
494 vector<double> lambdaMax(n);
496 // Initializes the Lagrange multipliers
498 for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, i++) {
499 lambda[i] = (*it)->value;
500 lambdaMax[i] = (*it)->Max;
501 lambdaMin[i] = (*it)->Min;
508 // Instruct the algorithm to zero out the relative movement between the
509 // aircraft and the ground.
510 vdot += (VState.vUVW - Tec2b * LocalTerrainVelocity) / dt;
511 wdot += (VState.vPQR - Tec2b * LocalTerrainAngularVelocity) / dt;
514 // Assemble the linear system of equations
515 for (i=0; i < n; i++) {
516 for (int j=0; j < i; j++)
517 a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of J^T*M^-1*J
518 for (int j=i; j < n; j++)
519 a[i*n+j] = DotProduct(JacF[i],invMass*JacF[j])+DotProduct(JacM[i],Jinv*JacM[j]);
522 // Prepare the linear system for the Gauss-Seidel algorithm :
523 // divide every line of 'a' and eta by a[i,i]. This is in order to save
524 // a division computation at each iteration of Gauss-Seidel.
525 for (i=0; i < n; i++) {
526 double d = 1.0 / a[i*n+i];
528 eta[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
529 for (int j=0; j < n; j++)
533 // Resolve the Lagrange multipliers with the projected Gauss-Seidel method
534 for (int iter=0; iter < 50; iter++) {
537 for (i=0; i < n; i++) {
538 double lambda0 = lambda[i];
539 double dlambda = eta[i];
541 for (int j=0; j < n; j++)
542 dlambda -= a[i*n+j]*lambda[j];
544 lambda[i] = Constrain(lambdaMin[i], lambda0+dlambda, lambdaMax[i]);
545 dlambda = lambda[i] - lambda0;
547 norm += fabs(dlambda);
550 if (norm < 1E-5) break;
553 // Calculate the total friction forces and moments
558 for (i=0; i< n; i++) {
559 Fc += lambda[i]*JacF[i];
560 Mc += lambda[i]*JacM[i];
563 vUVWdot += invMass * Fc;
564 vUVWidot += invMass * Tb2i * Fc;
565 vPQRdot += Jinv * Mc;
566 vPQRidot += Tb2i* Jinv * Mc;
568 // Save the value of the Lagrange multipliers to accelerate the convergence
569 // of the Gauss-Seidel algorithm at next iteration.
571 for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it)
572 (*it)->value = lambda[i++];
574 GroundReactions->UpdateForcesAndMoments();
577 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
579 void FGPropagate::UpdateLocationMatrices(void)
581 Tl2ec = GetTl2ec(); // local to ECEF transform
582 Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
584 Tl2i = Ti2l.Transposed();
587 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
589 void FGPropagate::UpdateBodyMatrices(void)
591 Ti2b = GetTi2b(); // ECI to body frame transform
592 Tb2i = Ti2b.Transposed(); // body to ECI frame transform
593 Tl2b = Ti2b*Tl2i; // local to body frame transform
594 Tb2l = Tl2b.Transposed(); // body to local frame transform
595 Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
596 Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
599 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
601 void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
602 VState.qAttitudeECI = Qi;
603 VState.qAttitudeECI.Normalize();
604 UpdateBodyMatrices();
605 VState.qAttitudeLocal = Tl2b.GetQuaternion();
608 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
610 void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
611 VState.vInertialVelocity = Vi;
613 vVel = GetTb2l() * VState.vUVW;
616 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
618 void FGPropagate::SetInertialRates(FGColumnVector3 vRates) {
619 VState.vPQRi_i = vRates;
620 VState.vPQRi = Ti2b * VState.vPQRi_i;
621 VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
624 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
626 void FGPropagate::InitializeDerivatives(void)
628 // Make an initial run and set past values
629 CalculatePQRdot(); // Angular rate derivative
630 CalculateUVWdot(); // Translational rate derivative
631 ResolveFrictionForces(0.); // Update rate derivatives with friction forces
632 CalculateQuatdot(); // Angular orientation derivative
633 CalculateInertialVelocity(); // Translational position derivative
635 // Initialize past values deques
636 VState.dqPQRdot.clear();
637 VState.dqPQRidot.clear();
638 VState.dqUVWidot.clear();
639 VState.dqInertialVelocity.clear();
640 VState.dqQtrndot.clear();
641 for (int i=0; i<4; i++) {
642 VState.dqPQRdot.push_front(vPQRdot);
643 VState.dqPQRidot.push_front(vPQRidot);
644 VState.dqUVWidot.push_front(vUVWdot);
645 VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
646 VState.dqQtrndot.push_front(vQtrndot);
650 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
652 void FGPropagate::RecomputeLocalTerrainRadius(void)
654 FGLocation contactloc;
656 double t = FDMExec->GetSimTime();
658 // Get the LocalTerrain radius.
659 FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv,
660 LocalTerrainVelocity, LocalTerrainAngularVelocity);
661 LocalTerrainRadius = contactloc.GetRadius();
664 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
666 void FGPropagate::SetTerrainElevation(double terrainElev)
668 LocalTerrainRadius = terrainElev + SeaLevelRadius;
669 FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
672 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
674 double FGPropagate::GetTerrainElevation(void) const
676 return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
679 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
680 //Todo: when should this be called - when should the new EPA be used to
681 // calculate the transformation matrix, so that the matrix is not a step
682 // ahead of the sim and the associated calculations?
683 const FGMatrix33& FGPropagate::GetTi2ec(void)
685 return VState.vLocation.GetTi2ec();
688 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
690 const FGMatrix33& FGPropagate::GetTec2i(void)
692 return VState.vLocation.GetTec2i();
695 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
697 void FGPropagate::SetAltitudeASL(double altASL)
699 VState.vLocation.SetRadius( altASL + SeaLevelRadius );
702 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
704 double FGPropagate::GetLocalTerrainRadius(void) const
706 return LocalTerrainRadius;
709 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
711 double FGPropagate::GetDistanceAGL(void) const
713 return VState.vLocation.GetRadius() - LocalTerrainRadius;
716 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
718 void FGPropagate::SetDistanceAGL(double tt)
720 VState.vLocation.SetRadius( tt + LocalTerrainRadius );
723 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
725 void FGPropagate::DumpState(void)
729 << "------------------------------------------------------------------" << reset << endl;
731 << "State Report at sim time: " << FDMExec->GetSimTime() << " seconds" << reset << endl;
732 cout << " " << underon
733 << "Position" << underoff << endl;
734 cout << " ECI: " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl;
735 cout << " ECEF: " << VState.vLocation << " (x,y,z, in ft)" << endl;
736 cout << " Local: " << VState.vLocation.GetLatitudeDeg()
737 << ", " << VState.vLocation.GetLongitudeDeg()
738 << ", " << GetAltitudeASL() << " (lat, lon, alt in deg and ft)" << endl;
740 cout << endl << " " << underon
741 << "Orientation" << underoff << endl;
742 cout << " ECI: " << VState.qAttitudeECI.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
743 cout << " Local: " << VState.qAttitudeLocal.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
745 cout << endl << " " << underon
746 << "Velocity" << underoff << endl;
747 cout << " ECI: " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl;
748 cout << " ECEF: " << (GetTb2ec() * VState.vUVW).Dump(", ") << " (x,y,z in ft/s)" << endl;
749 cout << " Local: " << GetVel() << " (n,e,d in ft/sec)" << endl;
750 cout << " Body: " << GetUVW() << " (u,v,w in ft/sec)" << endl;
752 cout << endl << " " << underon
753 << "Body Rates (relative to given frame, expressed in body frame)" << underoff << endl;
754 cout << " ECI: " << (VState.vPQRi*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
755 cout << " ECEF: " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
758 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
760 void FGPropagate::bind(void)
762 typedef double (FGPropagate::*PMF)(int) const;
764 PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
766 PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
767 PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
768 PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
770 PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
771 PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
772 PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
774 PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
775 PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
776 PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
778 PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
779 PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
780 PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
782 PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
784 PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
785 PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
786 PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
788 PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
789 PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
790 PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
792 PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
793 PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
794 PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
795 PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
796 PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
797 PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
798 PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
799 PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
800 PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
801 PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
802 PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
803 PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
804 &FGPropagate::GetTerrainElevation,
805 &FGPropagate::SetTerrainElevation, false);
807 PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
809 PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
810 PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
811 PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
813 PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
814 PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
815 PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
817 PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
818 PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
819 PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
820 PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
821 PropertyManager->Tie("simulation/gravity-model", &gravType);
824 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
825 // The bitmasked value choices are as follows:
826 // unset: In this case (the default) JSBSim would only print
827 // out the normally expected messages, essentially echoing
828 // the config files as they are read. If the environment
829 // variable is not set, debug_lvl is set to 1 internally
830 // 0: This requests JSBSim not to output any messages
832 // 1: This value explicity requests the normal JSBSim
834 // 2: This value asks for a message to be printed out when
835 // a class is instantiated
836 // 4: When this value is set, a message is displayed when a
837 // FGModel object executes its Run() method
838 // 8: When this value is set, various runtime state variables
839 // are printed out periodically
840 // 16: When set various parameters are sanity checked and
841 // a message is printed out when they go out of bounds
843 void FGPropagate::Debug(int from)
845 if (debug_lvl <= 0) return;
847 if (debug_lvl & 1) { // Standard console startup message output
848 if (from == 0) { // Constructor
852 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
853 if (from == 0) cout << "Instantiated: FGPropagate" << endl;
854 if (from == 1) cout << "Destroyed: FGPropagate" << endl;
856 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
858 if (debug_lvl & 8 && from == 2) { // Runtime state variables
859 cout << endl << fgblue << highint << left
860 << " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
863 cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
864 << Inertial->GetEarthPositionAngleDeg() << endl;
866 cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
867 cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
868 cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
869 cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
870 cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
871 cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
872 cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
873 cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
875 cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
876 << reset << endl << Tec2b.Dump("\t", " ") << endl;
877 cout << highint << " Associated Euler angles (deg): " << setw(8)
878 << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
881 cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
882 << reset << endl << Tb2ec.Dump("\t", " ") << endl;
883 cout << highint << " Associated Euler angles (deg): " << setw(8)
884 << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
887 cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
888 << reset << endl << Tl2b.Dump("\t", " ") << endl;
889 cout << highint << " Associated Euler angles (deg): " << setw(8)
890 << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
893 cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
894 << reset << endl << Tb2l.Dump("\t", " ") << endl;
895 cout << highint << " Associated Euler angles (deg): " << setw(8)
896 << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
899 cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
900 << reset << endl << Tl2ec.Dump("\t", " ") << endl;
901 cout << highint << " Associated Euler angles (deg): " << setw(8)
902 << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
905 cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
906 << reset << endl << Tec2l.Dump("\t", " ") << endl;
907 cout << highint << " Associated Euler angles (deg): " << setw(8)
908 << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
911 cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
912 << reset << endl << Tec2i.Dump("\t", " ") << endl;
913 cout << highint << " Associated Euler angles (deg): " << setw(8)
914 << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
917 cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
918 << reset << endl << Ti2ec.Dump("\t", " ") << endl;
919 cout << highint << " Associated Euler angles (deg): " << setw(8)
920 << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
923 cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
924 << reset << endl << Ti2b.Dump("\t", " ") << endl;
925 cout << highint << " Associated Euler angles (deg): " << setw(8)
926 << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
929 cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
930 << reset << endl << Tb2i.Dump("\t", " ") << endl;
931 cout << highint << " Associated Euler angles (deg): " << setw(8)
932 << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
935 cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
936 << reset << endl << Ti2l.Dump("\t", " ") << endl;
937 cout << highint << " Associated Euler angles (deg): " << setw(8)
938 << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
941 cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
942 << reset << endl << Tl2i.Dump("\t", " ") << endl;
943 cout << highint << " Associated Euler angles (deg): " << setw(8)
944 << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
947 cout << setprecision(6); // reset the output stream
949 if (debug_lvl & 16) { // Sanity checking
950 if (from == 2) { // State sanity checking
951 if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
952 cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
955 if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
956 cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
959 if (fabs(GetDistanceAGL()) > 1e10) {
960 cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
965 if (debug_lvl & 64) {
966 if (from == 0) { // Constructor
967 cout << IdSrc << endl;
968 cout << IdHdr << endl;