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] S. Buss, "Accurate and Efficient Simulation of Rigid Body Rotations",
52 Technical Report, Department of Mathematics, University of California,
54 [7] Barker L.E., Bowles R.L. and Williams L.H., "Development and Application of
55 a Local Linearization Algorithm for the Integration of Quaternion Rate
56 Equations in Real-Time Flight Simulation Problems", NASA TN D-7347,
58 [8] Phillips W.F, Hailey C.E and Gebert G.A, "Review of Attitude Representations
59 Used for Aircraft Kinematics", Journal Of Aircraft Vol. 38, No. 4,
62 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
64 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
71 #include "FGPropagate.h"
72 #include "FGGroundReactions.h"
73 #include "FGFDMExec.h"
74 #include "input_output/FGPropertyManager.h"
80 static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.105 2012/03/26 21:26:11 bcoconni Exp $";
81 static const char *IdHdr = ID_PROPAGATE;
83 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
85 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
87 FGPropagate::FGPropagate(FGFDMExec* fdmex)
94 vInertialVelocity.InitMatrix();
96 /// These define the indices use to select the various integrators.
97 // eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
99 integrator_rotational_rate = eRectEuler;
100 integrator_translational_rate = eAdamsBashforth2;
101 integrator_rotational_position = eRectEuler;
102 integrator_translational_position = eAdamsBashforth3;
104 VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
105 VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
106 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
107 VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0));
113 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
115 FGPropagate::~FGPropagate(void)
120 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
122 bool FGPropagate::InitModel(void)
124 // For initialization ONLY:
125 VState.vLocation.SetEllipse(in.SemiMajor, in.SemiMinor);
126 VState.vLocation.SetAltitudeAGL(4.0, FDMExec->GetSimTime());
128 vInertialVelocity.InitMatrix();
130 VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
131 VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
132 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
133 VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
135 integrator_rotational_rate = eRectEuler;
136 integrator_translational_rate = eAdamsBashforth2;
137 integrator_rotational_position = eRectEuler;
138 integrator_translational_position = eAdamsBashforth3;
143 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
145 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
147 // Initialize the State Vector elements and the transformation matrices
149 // Set the position lat/lon/radius
150 VState.vLocation = FGIC->GetPosition();
152 Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
153 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
155 VState.vInertialPosition = Tec2i * VState.vLocation;
157 UpdateLocationMatrices();
159 // Set the orientation from the euler angles (is normalized within the
160 // constructor). The Euler angles represent the orientation of the body
161 // frame relative to the local frame.
162 VState.qAttitudeLocal = FGIC->GetOrientation();
164 VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
165 UpdateBodyMatrices();
167 // Set the velocities in the instantaneus body frame
168 VState.vUVW = FGIC->GetUVWFpsIC();
170 // Compute the local frame ECEF velocity
171 vVel = Tb2l * VState.vUVW;
173 // Compute local terrain velocity
174 RecomputeLocalTerrainVelocity();
175 VehicleRadius = GetRadius();
177 // Set the angular velocities of the body frame relative to the ECEF frame,
178 // expressed in the body frame.
179 VState.vPQR = FGIC->GetPQRRadpsIC();
181 VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
183 CalculateInertialVelocity(); // Translational position derivative
186 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
187 // Initialize the past value deques
189 void FGPropagate::InitializeDerivatives()
191 for (int i=0; i<4; i++) {
192 VState.dqPQRidot[i] = in.vPQRidot;
193 VState.dqUVWidot[i] = in.vUVWidot;
194 VState.dqInertialVelocity[i] = VState.vInertialVelocity;
195 VState.dqQtrndot[i] = in.vQtrndot;
199 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
201 Purpose: Called on a schedule to perform EOM integration
202 Notes: [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
203 In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
205 At the top of this Run() function, see several "shortcuts" (or, aliases) being
206 set up for use later, rather than using the longer class->function() notation.
208 This propagation is done using the current state values
209 and current derivatives. Based on these values we compute an approximation to the
210 state values for (now + dt).
212 In the code below, variables named beginning with a small "v" refer to a
213 a column vector, variables named beginning with a "T" refer to a transformation
214 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
219 bool FGPropagate::Run(bool Holding)
221 if (FGModel::Run(Holding)) return true; // Fast return if we have nothing to do ...
222 if (Holding) return false;
224 double dt = in.DeltaT * rate; // The 'stepsize'
226 // Propagate rotational / translational velocity, angular /translational position, respectively.
228 Integrate(VState.qAttitudeECI, in.vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
229 Integrate(VState.vPQRi, in.vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate);
230 Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
231 Integrate(VState.vInertialVelocity, in.vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate);
233 // CAUTION : the order of the operations below is very important to get transformation
234 // matrices that are consistent with the new state of the vehicle
236 // 1. Update the Earth position angle (EPA)
237 VState.vLocation.IncrementEarthPositionAngle(in.vOmegaPlanet(eZ)*(in.DeltaT*rate));
239 // 2. Update the Ti2ec and Tec2i transforms from the updated EPA
240 Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
241 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
243 // 3. Update the location from the updated Ti2ec and inertial position
244 VState.vLocation = Ti2ec*VState.vInertialPosition;
246 // 4. Update the other "Location-based" transformation matrices from the updated
248 UpdateLocationMatrices();
250 // 5. Update the "Orientation-based" transformation matrices from the updated
251 // orientation quaternion and vLocation vector.
252 UpdateBodyMatrices();
254 // Translational position derivative (velocities are integrated in the inertial frame)
257 // Set auxilliary state variables
258 RecomputeLocalTerrainVelocity();
259 VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
261 VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
263 VState.qAttitudeLocal = Tl2b.GetQuaternion();
265 // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
266 vVel = Tb2l * VState.vUVW;
272 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
273 // Transform the velocity vector of the body relative to the origin (Earth
274 // center) to be expressed in the inertial frame, and add the vehicle velocity
275 // contribution due to the rotation of the planet.
276 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
277 // Second edition (2004), eqn 1.5-16c (page 50)
279 void FGPropagate::CalculateInertialVelocity(void)
281 VState.vInertialVelocity = Tb2i * VState.vUVW + (in.vOmegaPlanet * VState.vInertialPosition);
284 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
285 // Transform the velocity vector of the inertial frame to be expressed in the
286 // body frame relative to the origin (Earth center), and substract the vehicle
287 // velocity contribution due to the rotation of the planet.
289 void FGPropagate::CalculateUVW(void)
291 VState.vUVW = Ti2b * (VState.vInertialVelocity - (in.vOmegaPlanet * VState.vInertialPosition));
294 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
296 void FGPropagate::Integrate( FGColumnVector3& Integrand,
297 FGColumnVector3& Val,
298 deque <FGColumnVector3>& ValDot,
300 eIntegrateType integration_type)
302 ValDot.push_front(Val);
305 switch(integration_type) {
306 case eRectEuler: Integrand += dt*ValDot[0];
308 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
310 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
312 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
314 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
316 case eNone: // do nothing, freeze translational rate
323 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
325 void FGPropagate::Integrate( FGQuaternion& Integrand,
327 deque <FGQuaternion>& ValDot,
329 eIntegrateType integration_type)
331 ValDot.push_front(Val);
334 switch(integration_type) {
335 case eRectEuler: Integrand += dt*ValDot[0];
337 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
339 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
341 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
343 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
347 // This is the first order method as described in Samuel R. Buss paper[6].
348 // The formula from Buss' paper is transposed below to quaternions and is
349 // actually the exact solution of the quaternion differential equation
350 // qdot = 1/2*w*q when w is constant.
351 Integrand = Integrand * QExp(0.5 * dt * VState.vPQRi);
353 return; // No need to normalize since the quaternion exponential is always normal
356 // This is the 'augmented second-order method' from S.R. Buss paper [6].
357 // Unlike Runge-Kutta or Adams-Bashforth, it is a one-pass second-order
358 // method (see reference [6]).
359 FGColumnVector3 wi = VState.vPQRi;
360 FGColumnVector3 wdoti = in.vPQRidot;
361 FGColumnVector3 omega = wi + 0.5*dt*wdoti + dt*dt/12.*wdoti*wi;
362 Integrand = Integrand * QExp(0.5 * dt * omega);
364 return; // No need to normalize since the quaternion exponential is always normal
365 case eLocalLinearization:
367 // This is the local linearization algorithm of Barker et al. (see ref. [7])
368 // It is also a one-pass second-order method. The code below is based on the
369 // more compact formulation issued from equation (107) of ref. [8]. The
370 // constants C1, C2, C3 and C4 have the same value than those in ref. [7] pp. 11
371 FGColumnVector3 wi = 0.5 * VState.vPQRi;
372 FGColumnVector3 wdoti = 0.5 * in.vPQRidot;
373 double omegak2 = DotProduct(VState.vPQRi, VState.vPQRi);
374 double omegak = omegak2 > 1E-6 ? sqrt(omegak2) : 1E-6;
375 double rhok = 0.5 * dt * omegak;
376 double C1 = cos(rhok);
377 double C2 = 2.0 * sin(rhok) / omegak;
378 double C3 = 4.0 * (1.0 - C1) / (omegak*omegak);
379 double C4 = 4.0 * (dt - C2) / (omegak*omegak);
380 FGColumnVector3 Omega = C2*wi + C3*wdoti + C4*wi*wdoti;
383 q(1) = C1 - C4*DotProduct(wi, wdoti);
388 Integrand = Integrand * q;
390 /* Cross check with ref. [7] pp.11-12 formulas and code pp. 20
391 double pk = VState.vPQRi(eP);
392 double qk = VState.vPQRi(eQ);
393 double rk = VState.vPQRi(eR);
394 double pdotk = in.vPQRidot(eP);
395 double qdotk = in.vPQRidot(eQ);
396 double rdotk = in.vPQRidot(eR);
397 double Ap = -0.25 * (pk*pdotk + qk*qdotk + rk*rdotk);
398 double Bp = 0.25 * (pk*qdotk - qk*pdotk);
399 double Cp = 0.25 * (pdotk*rk - pk*rdotk);
400 double Dp = 0.25 * (qk*rdotk - qdotk*rk);
401 double C2p = sin(rhok) / omegak;
402 double C3p = 2.0 * (1.0 - cos(rhok)) / (omegak*omegak);
403 double H = C1 + C4 * Ap;
404 double G = -C2p*rk - C3p*rdotk + C4*Bp;
405 double J = C2p*qk + C3p*qdotk - C4*Cp;
406 double K = C2p*pk + C3p*pdotk - C4*Dp;
408 cout << "q: " << q << endl;
410 // Warning! In the paper of Barker et al. the quaternion components are not
411 // ordered the same way as in JSBSim (see equations (2) and (3) of ref. [7]
412 // as well as the comment just below equation (3))
413 cout << "FORTRAN: " << H << " , " << K << " , " << J << " , " << -G << endl;*/
415 break; // The quaternion q is not normal so the normalization needs to be done.
416 case eNone: // do nothing, freeze rotational rate
422 Integrand.Normalize();
425 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
427 void FGPropagate::UpdateLocationMatrices(void)
429 Tl2ec = VState.vLocation.GetTl2ec(); // local to ECEF transform
430 Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
431 Ti2l = VState.vLocation.GetTi2l(); // ECI to local frame transform
432 Tl2i = Ti2l.Transposed(); // local to ECI transform
435 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
437 void FGPropagate::UpdateBodyMatrices(void)
439 Ti2b = VState.qAttitudeECI.GetT(); // ECI to body frame transform
440 Tb2i = Ti2b.Transposed(); // body to ECI frame transform
441 Tl2b = Ti2b * Tl2i; // local to body frame transform
442 Tb2l = Tl2b.Transposed(); // body to local frame transform
443 Tec2b = Ti2b * Tec2i; // ECEF to body frame transform
444 Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
447 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
449 void FGPropagate::SetInertialOrientation(const FGQuaternion& Qi) {
450 VState.qAttitudeECI = Qi;
451 VState.qAttitudeECI.Normalize();
452 UpdateBodyMatrices();
453 VState.qAttitudeLocal = Tl2b.GetQuaternion();
456 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
458 void FGPropagate::SetInertialVelocity(const FGColumnVector3& Vi) {
459 VState.vInertialVelocity = Vi;
461 vVel = Tb2l * VState.vUVW;
464 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
466 void FGPropagate::SetInertialRates(const FGColumnVector3& vRates) {
467 VState.vPQRi = Ti2b * vRates;
468 VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
471 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
473 void FGPropagate::RecomputeLocalTerrainVelocity()
476 FGColumnVector3 normal;
477 VState.vLocation.GetContactPoint(FDMExec->GetSimTime(), contact, normal,
478 LocalTerrainVelocity, LocalTerrainAngularVelocity);
481 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
483 void FGPropagate::SetTerrainElevation(double terrainElev)
485 double radius = terrainElev + VState.vLocation.GetSeaLevelRadius();
486 FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(radius);
490 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
492 void FGPropagate::SetSeaLevelRadius(double tt)
494 FDMExec->GetGroundCallback()->SetSeaLevelRadius(tt);
497 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
499 double FGPropagate::GetLocalTerrainRadius(void) const
501 return VState.vLocation.GetTerrainRadius(FDMExec->GetSimTime());
504 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
506 double FGPropagate::GetDistanceAGL(void) const
508 return VState.vLocation.GetAltitudeAGL(FDMExec->GetSimTime());
511 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
513 void FGPropagate::SetDistanceAGL(double tt)
515 VState.vLocation.SetAltitudeAGL(tt, FDMExec->GetSimTime());
516 UpdateVehicleState();
519 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
521 void FGPropagate::SetVState(const VehicleState& vstate)
523 //ToDo: Shouldn't all of these be set from the vstate vector passed in?
524 VState.vLocation = vstate.vLocation;
525 Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
526 Tec2i = Ti2ec.Transposed();
527 UpdateLocationMatrices();
528 SetInertialOrientation(vstate.qAttitudeECI);
529 RecomputeLocalTerrainVelocity();
530 VehicleRadius = GetRadius();
531 VState.vUVW = vstate.vUVW;
532 vVel = Tb2l * VState.vUVW;
533 VState.vPQR = vstate.vPQR;
534 VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
535 VState.vInertialPosition = vstate.vInertialPosition;
538 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
540 void FGPropagate::UpdateVehicleState(void)
542 RecomputeLocalTerrainVelocity();
543 VehicleRadius = GetRadius();
544 VState.vInertialPosition = Tec2i * VState.vLocation;
545 UpdateLocationMatrices();
546 UpdateBodyMatrices();
547 vVel = Tb2l * VState.vUVW;
548 VState.qAttitudeLocal = Tl2b.GetQuaternion();
551 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
553 void FGPropagate::SetLocation(const FGLocation& l)
555 VState.vLocation = l;
556 Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
557 Tec2i = Ti2ec.Transposed();
558 UpdateVehicleState();
561 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
563 void FGPropagate::DumpState(void)
567 << "------------------------------------------------------------------" << reset << endl;
569 << "State Report at sim time: " << FDMExec->GetSimTime() << " seconds" << reset << endl;
570 cout << " " << underon
571 << "Position" << underoff << endl;
572 cout << " ECI: " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl;
573 cout << " ECEF: " << VState.vLocation << " (x,y,z, in ft)" << endl;
574 cout << " Local: " << VState.vLocation.GetLatitudeDeg()
575 << ", " << VState.vLocation.GetLongitudeDeg()
576 << ", " << GetAltitudeASL() << " (lat, lon, alt in deg and ft)" << endl;
578 cout << endl << " " << underon
579 << "Orientation" << underoff << endl;
580 cout << " ECI: " << VState.qAttitudeECI.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
581 cout << " Local: " << VState.qAttitudeLocal.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
583 cout << endl << " " << underon
584 << "Velocity" << underoff << endl;
585 cout << " ECI: " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl;
586 cout << " ECEF: " << (Tb2ec * VState.vUVW).Dump(", ") << " (x,y,z in ft/s)" << endl;
587 cout << " Local: " << GetVel() << " (n,e,d in ft/sec)" << endl;
588 cout << " Body: " << GetUVW() << " (u,v,w in ft/sec)" << endl;
590 cout << endl << " " << underon
591 << "Body Rates (relative to given frame, expressed in body frame)" << underoff << endl;
592 cout << " ECI: " << (VState.vPQRi*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
593 cout << " ECEF: " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
596 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
598 void FGPropagate::bind(void)
600 typedef double (FGPropagate::*PMF)(int) const;
602 PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
604 PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
605 PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
606 PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
608 PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
609 PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
610 PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
612 PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
613 PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
614 PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
616 PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
617 PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
618 PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
620 PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
622 PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
623 PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
624 PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude, false);
625 PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude, false);
626 PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg, false);
627 PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg, false);
628 PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
629 PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
630 PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
631 PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
632 PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
633 PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
634 &FGPropagate::GetTerrainElevation,
635 &FGPropagate::SetTerrainElevation, false);
637 PropertyManager->Tie("position/epa-rad", this, &FGPropagate::GetEarthPositionAngle);
638 PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
640 PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
641 PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
642 PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
644 PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
645 PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
646 PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
648 PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
649 PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
650 PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
651 PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
654 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
655 // The bitmasked value choices are as follows:
656 // unset: In this case (the default) JSBSim would only print
657 // out the normally expected messages, essentially echoing
658 // the config files as they are read. If the environment
659 // variable is not set, debug_lvl is set to 1 internally
660 // 0: This requests JSBSim not to output any messages
662 // 1: This value explicity requests the normal JSBSim
664 // 2: This value asks for a message to be printed out when
665 // a class is instantiated
666 // 4: When this value is set, a message is displayed when a
667 // FGModel object executes its Run() method
668 // 8: When this value is set, various runtime state variables
669 // are printed out periodically
670 // 16: When set various parameters are sanity checked and
671 // a message is printed out when they go out of bounds
673 void FGPropagate::Debug(int from)
675 if (debug_lvl <= 0) return;
677 if (debug_lvl & 1) { // Standard console startup message output
678 if (from == 0) { // Constructor
682 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
683 if (from == 0) cout << "Instantiated: FGPropagate" << endl;
684 if (from == 1) cout << "Destroyed: FGPropagate" << endl;
686 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
688 if (debug_lvl & 8 && from == 2) { // Runtime state variables
689 cout << endl << fgblue << highint << left
690 << " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
693 cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
694 << GetEarthPositionAngleDeg() << endl;
696 cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
697 cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
698 cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
699 cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
700 cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
701 cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
702 cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
703 // cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
705 cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
706 << reset << endl << Tec2b.Dump("\t", " ") << endl;
707 cout << highint << " Associated Euler angles (deg): " << setw(8)
708 << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
711 cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
712 << reset << endl << Tb2ec.Dump("\t", " ") << endl;
713 cout << highint << " Associated Euler angles (deg): " << setw(8)
714 << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
717 cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
718 << reset << endl << Tl2b.Dump("\t", " ") << endl;
719 cout << highint << " Associated Euler angles (deg): " << setw(8)
720 << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
723 cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
724 << reset << endl << Tb2l.Dump("\t", " ") << endl;
725 cout << highint << " Associated Euler angles (deg): " << setw(8)
726 << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
729 cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
730 << reset << endl << Tl2ec.Dump("\t", " ") << endl;
731 cout << highint << " Associated Euler angles (deg): " << setw(8)
732 << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
735 cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
736 << reset << endl << Tec2l.Dump("\t", " ") << endl;
737 cout << highint << " Associated Euler angles (deg): " << setw(8)
738 << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
741 cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
742 << reset << endl << Tec2i.Dump("\t", " ") << endl;
743 cout << highint << " Associated Euler angles (deg): " << setw(8)
744 << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
747 cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
748 << reset << endl << Ti2ec.Dump("\t", " ") << endl;
749 cout << highint << " Associated Euler angles (deg): " << setw(8)
750 << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
753 cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
754 << reset << endl << Ti2b.Dump("\t", " ") << endl;
755 cout << highint << " Associated Euler angles (deg): " << setw(8)
756 << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
759 cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
760 << reset << endl << Tb2i.Dump("\t", " ") << endl;
761 cout << highint << " Associated Euler angles (deg): " << setw(8)
762 << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
765 cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
766 << reset << endl << Ti2l.Dump("\t", " ") << endl;
767 cout << highint << " Associated Euler angles (deg): " << setw(8)
768 << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
771 cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
772 << reset << endl << Tl2i.Dump("\t", " ") << endl;
773 cout << highint << " Associated Euler angles (deg): " << setw(8)
774 << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
777 cout << setprecision(6); // reset the output stream
779 if (debug_lvl & 16) { // Sanity checking
780 if (from == 2) { // State sanity checking
781 if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
782 cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
785 if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
786 cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
789 if (fabs(GetDistanceAGL()) > 1e10) {
790 cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
795 if (debug_lvl & 64) {
796 if (from == 0) { // Constructor
797 cout << IdSrc << endl;
798 cout << IdHdr << endl;