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.60 2010/08/12 19:11:22 andgi 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.dqUVWdot.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 = Inertial->GetRefRadius();
122 VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
123 VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
124 vOmegaEarth = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
126 vPQRdot.InitMatrix();
127 vQtrndot = FGQuaternion(0,0,0);
128 vUVWdot.InitMatrix();
129 vInertialVelocity.InitMatrix();
131 VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
132 VState.dqUVWdot.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 VehicleRadius = GetRadius();
152 radInv = 1.0/VehicleRadius;
154 // Initialize the State Vector elements and the transformation matrices
156 // Set the position lat/lon/radius
157 VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
158 FGIC->GetLatitudeRadIC(),
159 FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
161 VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle());
163 Ti2ec = GetTi2ec(); // ECI to ECEF transform
164 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
166 Tl2ec = GetTl2ec(); // local to ECEF transform
167 Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
170 Tl2i = Ti2l.Transposed();
172 // Set the orientation from the euler angles (is normalized within the
173 // constructor). The Euler angles represent the orientation of the body
174 // frame relative to the local frame.
175 VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(),
176 FGIC->GetThetaRadIC(),
177 FGIC->GetPsiRadIC() );
179 VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
181 Ti2b = GetTi2b(); // ECI to body frame transform
182 Tb2i = Ti2b.Transposed(); // body to ECI frame transform
184 Tl2b = VState.qAttitudeLocal; // local to body frame transform
185 Tb2l = Tl2b.Transposed(); // body to local frame transform
187 Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
188 Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
190 // Set the velocities in the instantaneus body frame
191 VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
192 FGIC->GetVBodyFpsIC(),
193 FGIC->GetWBodyFpsIC() );
195 VState.vInertialPosition = Tec2i * VState.vLocation;
197 // Compute the local frame ECEF velocity
198 vVel = Tb2l * VState.vUVW;
200 // Refer to Stevens and Lewis, 1.5-14a, pg. 49.
201 // This is the rotation rate of the "Local" frame, expressed in the local frame.
203 FGColumnVector3 vOmegaLocal = FGColumnVector3(
205 -radInv*vVel(eNorth),
206 -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
208 // Set the angular velocities of the body frame relative to the ECEF frame,
209 // expressed in the body frame. Effectively, this is:
210 // w_b/e = w_b/l + w_l/e
211 VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
213 FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal;
215 VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
217 // Make an initial run and set past values
218 CalculatePQRdot(); // Angular rate derivative
219 CalculateUVWdot(); // Translational rate derivative
220 ResolveFrictionForces(0.); // Update rate derivatives with friction forces
221 CalculateQuatdot(); // Angular orientation derivative
222 CalculateInertialVelocity(); // Translational position derivative
224 // Initialize past values deques
225 VState.dqPQRdot.clear();
226 VState.dqUVWdot.clear();
227 VState.dqInertialVelocity.clear();
228 VState.dqQtrndot.clear();
229 for (int i=0; i<4; i++) {
230 VState.dqPQRdot.push_front(vPQRdot);
231 VState.dqUVWdot.push_front(vUVWdot);
232 VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
233 VState.dqQtrndot.push_front(vQtrndot);
236 // Recompute the LocalTerrainRadius.
237 RecomputeLocalTerrainRadius();
240 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
242 Purpose: Called on a schedule to perform EOM integration
243 Notes: [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
244 In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
246 At the top of this Run() function, see several "shortcuts" (or, aliases) being
247 set up for use later, rather than using the longer class->function() notation.
249 This propagation is done using the current state values
250 and current derivatives. Based on these values we compute an approximation to the
251 state values for (now + dt).
253 In the code below, variables named beginning with a small "v" refer to a
254 a column vector, variables named beginning with a "T" refer to a transformation
255 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
260 bool FGPropagate::Run(void)
262 if (FGModel::Run()) return true; // Fast return if we have nothing to do ...
263 if (FDMExec->Holding()) return false;
265 double dt = FDMExec->GetDeltaT()*rate; // The 'stepsize'
269 // Calculate state derivatives
270 CalculatePQRdot(); // Angular rate derivative
271 CalculateUVWdot(); // Translational rate derivative
272 ResolveFrictionForces(dt); // Update rate derivatives with friction forces
273 CalculateQuatdot(); // Angular orientation derivative
274 CalculateInertialVelocity(); // Translational position derivative
276 // Propagate rotational / translational velocity, angular /translational position, respectively.
277 Integrate(VState.vPQRi, vPQRdot, VState.dqPQRdot, dt, integrator_rotational_rate);
278 Integrate(VState.vUVW, vUVWdot, VState.dqUVWdot, dt, integrator_translational_rate);
279 Integrate(VState.qAttitudeECI, vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
280 Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
282 VState.qAttitudeECI.Normalize(); // Normalize the ECI Attitude quaternion
284 VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle()); // Update the Earth position angle (EPA)
286 // Update the "Location-based" transformation matrices from the vLocation vector.
288 Ti2ec = GetTi2ec(); // ECI to ECEF transform
289 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
290 Tl2ec = GetTl2ec(); // local to ECEF transform
291 Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
293 Tl2i = Ti2l.Transposed();
295 // Update the "Orientation-based" transformation matrices from the orientation quaternion
297 Ti2b = GetTi2b(); // ECI to body frame transform
298 Tb2i = Ti2b.Transposed(); // body to ECI frame transform
299 Tl2b = Ti2b*Tl2i; // local to body frame transform
300 Tb2l = Tl2b.Transposed(); // body to local frame transform
301 Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
302 Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
304 // Set auxililary state variables
305 VState.vLocation = Ti2ec*VState.vInertialPosition;
306 RecomputeLocalTerrainRadius();
308 VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
309 radInv = 1.0/VehicleRadius;
311 VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
313 VState.qAttitudeLocal = Tl2b.GetQuaternion();
315 // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
316 vVel = Tb2l * VState.vUVW;
324 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
325 // Compute body frame rotational accelerations based on the current body moments
327 // vPQRdot is the derivative of the absolute angular velocity of the vehicle
328 // (body rate with respect to the inertial frame), expressed in the body frame,
329 // where the derivative is taken in the body frame.
330 // J is the inertia matrix
331 // Jinv is the inverse inertia matrix
332 // vMoments is the moment vector in the body frame
333 // VState.vPQRi is the total inertial angular velocity of the vehicle
334 // expressed in the body frame.
335 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
336 // Second edition (2004), eqn 1.5-16e (page 50)
338 void FGPropagate::CalculatePQRdot(void)
340 const FGColumnVector3& vMoments = Aircraft->GetMoments(); // current moments
341 const FGMatrix33& J = MassBalance->GetJ(); // inertia matrix
342 const FGMatrix33& Jinv = MassBalance->GetJinv(); // inertia matrix inverse
344 // Compute body frame rotational accelerations based on the current body
345 // moments and the total inertial angular velocity expressed in the body
348 vPQRdot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
351 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
352 // Compute the quaternion orientation derivative
354 // vQtrndot is the quaternion derivative.
355 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
356 // Second edition (2004), eqn 1.5-16b (page 50)
358 void FGPropagate::CalculateQuatdot(void)
360 // Compute quaternion orientation derivative on current body rates
361 vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi);
364 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
365 // This set of calculations results in the body frame accelerations being
367 // Compute body frame accelerations based on the current body forces.
368 // Include centripetal and coriolis accelerations.
369 // vOmegaEarth is the Earth angular rate - expressed in the inertial frame -
370 // so it has to be transformed to the body frame. More completely,
371 // vOmegaEarth is the rate of the ECEF frame relative to the Inertial
372 // frame (ECI), expressed in the Inertial frame.
373 // vForces is the total force on the vehicle in the body frame.
374 // VState.vPQR is the vehicle body rate relative to the ECEF frame, expressed
375 // in the body frame.
376 // VState.vUVW is the vehicle velocity relative to the ECEF frame, expressed
377 // in the body frame.
378 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
379 // Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
381 void FGPropagate::CalculateUVWdot(void)
383 double mass = MassBalance->GetMass(); // mass
384 const FGColumnVector3& vForces = Aircraft->GetForces(); // current forces
386 vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
388 // Include Centripetal acceleration.
389 vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
391 // Include Gravitation accel
394 vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) );
397 vGravAccel = Tec2b * Inertial->GetGravityJ2(VState.vLocation);
401 vUVWdot += vGravAccel;
404 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
405 // Transform the velocity vector of the body relative to the origin (Earth
406 // center) to be expressed in the inertial frame, and add the vehicle velocity
407 // contribution due to the rotation of the planet.
408 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
409 // Second edition (2004), eqn 1.5-16c (page 50)
411 void FGPropagate::CalculateInertialVelocity(void)
413 VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
416 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
418 void FGPropagate::Integrate( FGColumnVector3& Integrand,
419 FGColumnVector3& Val,
420 deque <FGColumnVector3>& ValDot,
422 eIntegrateType integration_type)
424 ValDot.push_front(Val);
427 switch(integration_type) {
428 case eRectEuler: Integrand += dt*ValDot[0];
430 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
432 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
434 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
436 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
438 case eNone: // do nothing, freeze translational rate
443 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
445 void FGPropagate::Integrate( FGQuaternion& Integrand,
447 deque <FGQuaternion>& ValDot,
449 eIntegrateType integration_type)
451 ValDot.push_front(Val);
454 switch(integration_type) {
455 case eRectEuler: Integrand += dt*ValDot[0];
457 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
459 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
461 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
463 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
465 case eNone: // do nothing, freeze rotational rate
470 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
471 // Resolves the contact forces just before integrating the EOM.
472 // This routine is using Lagrange multipliers and the projected Gauss-Seidel
474 // Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence",
476 // In JSBSim there is only one rigid body (the aircraft) and there can be
477 // multiple points of contact between the aircraft and the ground. As a
478 // consequence our matrix J*M^-1*J^T is not sparse and the algorithm described
479 // in Catto's paper has been adapted accordingly.
481 void FGPropagate::ResolveFrictionForces(double dt)
483 const double invMass = 1.0 / MassBalance->GetMass();
484 const FGMatrix33& Jinv = MassBalance->GetJinv();
485 vector <FGColumnVector3> JacF, JacM;
486 FGColumnVector3 vdot, wdot;
487 FGColumnVector3 Fc, Mc;
490 // Compiles data from the ground reactions to build up the jacobian matrix
491 for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, n++) {
492 JacF.push_back((*it)->ForceJacobian);
493 JacM.push_back((*it)->MomentJacobian);
496 // If no gears are in contact with the ground then return
499 vector<double> a(n*n); // Will contain J*M^-1*J^T
500 vector<double> eta(n);
501 vector<double> lambda(n);
502 vector<double> lambdaMin(n);
503 vector<double> lambdaMax(n);
505 // Initializes the Lagrange multipliers
507 for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, i++) {
508 lambda[i] = (*it)->value;
509 lambdaMax[i] = (*it)->Max;
510 lambdaMin[i] = (*it)->Min;
517 // First compute the ground velocity below the aircraft center of gravity
519 FGColumnVector3 normal, cvel;
521 // Instruct the algorithm to zero out the relative movement between the
522 // aircraft and the ground.
523 vdot += (VState.vUVW - Tec2b * cvel) / dt;
524 wdot += VState.vPQR / dt;
527 // Assemble the linear system of equations
528 for (i=0; i < n; i++) {
529 for (int j=0; j < i; j++)
530 a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of J^T*M^-1*J
531 for (int j=i; j < n; j++)
532 a[i*n+j] = DotProduct(JacF[i],invMass*JacF[j])+DotProduct(JacM[i],Jinv*JacM[j]);
535 // Prepare the linear system for the Gauss-Seidel algorithm :
536 // divide every line of 'a' and eta by a[i,i]. This is in order to save
537 // a division computation at each iteration of Gauss-Seidel.
538 for (i=0; i < n; i++) {
539 double d = 1.0 / a[i*n+i];
541 eta[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
542 for (int j=0; j < n; j++)
546 // Resolve the Lagrange multipliers with the projected Gauss-Seidel method
547 for (int iter=0; iter < 50; iter++) {
550 for (i=0; i < n; i++) {
551 double lambda0 = lambda[i];
552 double dlambda = eta[i];
554 for (int j=0; j < n; j++)
555 dlambda -= a[i*n+j]*lambda[j];
557 lambda[i] = Constrain(lambdaMin[i], lambda0+dlambda, lambdaMax[i]);
558 dlambda = lambda[i] - lambda0;
560 norm += fabs(dlambda);
563 if (norm < 1E-5) break;
566 // Calculate the total friction forces and moments
571 for (i=0; i< n; i++) {
572 Fc += lambda[i]*JacF[i];
573 Mc += lambda[i]*JacM[i];
576 vUVWdot += invMass * Fc;
577 vPQRdot += Jinv * Mc;
579 // Save the value of the Lagrange multipliers to accelerate the convergence
580 // of the Gauss-Seidel algorithm at next iteration.
582 for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it)
583 (*it)->value = lambda[i++];
585 GroundReactions->UpdateForcesAndMoments();
588 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
590 void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
591 VState.qAttitudeECI = Qi;
594 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
596 void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
597 VState.vInertialVelocity = Vi;
600 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
602 void FGPropagate::RecomputeLocalTerrainRadius(void)
604 double t = FDMExec->GetSimTime();
606 // Get the LocalTerrain radius.
607 FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
608 LocalTerrainRadius = contactloc.GetRadius();
611 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
613 void FGPropagate::SetTerrainElevation(double terrainElev)
615 LocalTerrainRadius = terrainElev + SeaLevelRadius;
616 FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
619 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
621 double FGPropagate::GetTerrainElevation(void) const
623 return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
626 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
627 //Todo: when should this be called - when should the new EPA be used to
628 // calculate the transformation matrix, so that the matrix is not a step
629 // ahead of the sim and the associated calculations?
630 const FGMatrix33& FGPropagate::GetTi2ec(void)
632 return VState.vLocation.GetTi2ec();
635 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
637 const FGMatrix33& FGPropagate::GetTec2i(void)
639 return VState.vLocation.GetTec2i();
642 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
644 void FGPropagate::SetAltitudeASL(double altASL)
646 VState.vLocation.SetRadius( altASL + SeaLevelRadius );
649 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
651 double FGPropagate::GetLocalTerrainRadius(void) const
653 return LocalTerrainRadius;
656 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
658 double FGPropagate::GetDistanceAGL(void) const
660 return VState.vLocation.GetRadius() - LocalTerrainRadius;
663 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
665 void FGPropagate::SetDistanceAGL(double tt)
667 VState.vLocation.SetRadius( tt + LocalTerrainRadius );
670 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
672 void FGPropagate::bind(void)
674 typedef double (FGPropagate::*PMF)(int) const;
676 PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
678 PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
679 PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
680 PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
682 PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
683 PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
684 PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
686 PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
687 PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
688 PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
690 PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
691 PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
692 PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
694 PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
696 PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
697 PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
698 PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
700 PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
701 PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
702 PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
704 PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
705 PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
706 PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
707 PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
708 PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
709 PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
710 PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
711 PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
712 PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
713 PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
714 PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
715 PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
716 &FGPropagate::GetTerrainElevation,
717 &FGPropagate::SetTerrainElevation, false);
719 PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
721 PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
722 PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
723 PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
725 PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
726 PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
727 PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
729 PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
730 PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
731 PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
732 PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
733 PropertyManager->Tie("simulation/gravity-model", &gravType);
736 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
737 // The bitmasked value choices are as follows:
738 // unset: In this case (the default) JSBSim would only print
739 // out the normally expected messages, essentially echoing
740 // the config files as they are read. If the environment
741 // variable is not set, debug_lvl is set to 1 internally
742 // 0: This requests JSBSim not to output any messages
744 // 1: This value explicity requests the normal JSBSim
746 // 2: This value asks for a message to be printed out when
747 // a class is instantiated
748 // 4: When this value is set, a message is displayed when a
749 // FGModel object executes its Run() method
750 // 8: When this value is set, various runtime state variables
751 // are printed out periodically
752 // 16: When set various parameters are sanity checked and
753 // a message is printed out when they go out of bounds
755 void FGPropagate::Debug(int from)
757 if (debug_lvl <= 0) return;
759 if (debug_lvl & 1) { // Standard console startup message output
760 if (from == 0) { // Constructor
764 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
765 if (from == 0) cout << "Instantiated: FGPropagate" << endl;
766 if (from == 1) cout << "Destroyed: FGPropagate" << endl;
768 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
770 if (debug_lvl & 8 && from == 2) { // Runtime state variables
771 cout << endl << fgblue << highint << left
772 << " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
775 cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
776 << Inertial->GetEarthPositionAngleDeg() << endl;
778 cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
779 cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
780 cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
781 cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
782 cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
783 cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
784 cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
785 cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
787 cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
788 << reset << endl << Tec2b.Dump("\t", " ") << endl;
789 cout << highint << " Associated Euler angles (deg): " << setw(8)
790 << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
793 cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
794 << reset << endl << Tb2ec.Dump("\t", " ") << endl;
795 cout << highint << " Associated Euler angles (deg): " << setw(8)
796 << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
799 cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
800 << reset << endl << Tl2b.Dump("\t", " ") << endl;
801 cout << highint << " Associated Euler angles (deg): " << setw(8)
802 << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
805 cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
806 << reset << endl << Tb2l.Dump("\t", " ") << endl;
807 cout << highint << " Associated Euler angles (deg): " << setw(8)
808 << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
811 cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
812 << reset << endl << Tl2ec.Dump("\t", " ") << endl;
813 cout << highint << " Associated Euler angles (deg): " << setw(8)
814 << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
817 cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
818 << reset << endl << Tec2l.Dump("\t", " ") << endl;
819 cout << highint << " Associated Euler angles (deg): " << setw(8)
820 << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
823 cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
824 << reset << endl << Tec2i.Dump("\t", " ") << endl;
825 cout << highint << " Associated Euler angles (deg): " << setw(8)
826 << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
829 cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
830 << reset << endl << Ti2ec.Dump("\t", " ") << endl;
831 cout << highint << " Associated Euler angles (deg): " << setw(8)
832 << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
835 cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
836 << reset << endl << Ti2b.Dump("\t", " ") << endl;
837 cout << highint << " Associated Euler angles (deg): " << setw(8)
838 << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
841 cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
842 << reset << endl << Tb2i.Dump("\t", " ") << endl;
843 cout << highint << " Associated Euler angles (deg): " << setw(8)
844 << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
847 cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
848 << reset << endl << Ti2l.Dump("\t", " ") << endl;
849 cout << highint << " Associated Euler angles (deg): " << setw(8)
850 << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
853 cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
854 << reset << endl << Tl2i.Dump("\t", " ") << endl;
855 cout << highint << " Associated Euler angles (deg): " << setw(8)
856 << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
859 cout << setprecision(6); // reset the output stream
861 if (debug_lvl & 16) { // Sanity checking
862 if (from == 2) { // State sanity checking
863 if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
864 cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
867 if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
868 cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
871 if (fabs(GetDistanceAGL()) > 1e10) {
872 cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
877 if (debug_lvl & 64) {
878 if (from == 0) { // Constructor
879 cout << IdSrc << endl;
880 cout << IdHdr << endl;