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
52 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
54 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
61 #include "FGPropagate.h"
62 #include "FGGroundReactions.h"
63 #include "FGFDMExec.h"
64 #include "FGAircraft.h"
65 #include "FGMassBalance.h"
66 #include "FGInertial.h"
67 #include "input_output/FGPropertyManager.h"
73 static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.55 2010/07/09 04:11:45 jberndt Exp $";
74 static const char *IdHdr = ID_PROPAGATE;
76 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
78 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
80 FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
87 vQtrndot = FGQuaternion(0,0,0);
89 vInertialVelocity.InitMatrix();
91 integrator_rotational_rate = eAdamsBashforth2;
92 integrator_translational_rate = eTrapezoidal;
93 integrator_rotational_position = eAdamsBashforth2;
94 integrator_translational_position = eTrapezoidal;
96 VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
97 VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
98 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
99 VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0));
105 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
107 FGPropagate::~FGPropagate(void)
112 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
114 bool FGPropagate::InitModel(void)
116 if (!FGModel::InitModel()) return false;
118 // For initialization ONLY:
119 SeaLevelRadius = LocalTerrainRadius = Inertial->GetRefRadius();
121 VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
122 VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
123 vOmegaEarth = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
125 vPQRdot.InitMatrix();
126 vQtrndot = FGQuaternion(0,0,0);
127 vUVWdot.InitMatrix();
128 vInertialVelocity.InitMatrix();
130 VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
131 VState.dqUVWdot.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 = eAdamsBashforth2;
136 integrator_translational_rate = eTrapezoidal;
137 integrator_rotational_position = eAdamsBashforth2;
138 integrator_translational_position = eTrapezoidal;
143 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
145 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
147 SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
148 SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
150 VehicleRadius = GetRadius();
151 radInv = 1.0/VehicleRadius;
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 Tl2ec = GetTl2ec(); // local to ECEF transform
166 Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
169 Tl2i = Ti2l.Transposed();
171 // Set the orientation from the euler angles (is normalized within the
172 // constructor). The Euler angles represent the orientation of the body
173 // frame relative to the local frame.
174 VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(),
175 FGIC->GetThetaRadIC(),
176 FGIC->GetPsiRadIC() );
178 VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
180 Ti2b = GetTi2b(); // ECI to body frame transform
181 Tb2i = Ti2b.Transposed(); // body to ECI frame transform
183 Tl2b = VState.qAttitudeLocal; // local to body frame transform
184 Tb2l = Tl2b.Transposed(); // body to local frame transform
186 Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
187 Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
189 // Set the velocities in the instantaneus body frame
190 VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
191 FGIC->GetVBodyFpsIC(),
192 FGIC->GetWBodyFpsIC() );
194 VState.vInertialPosition = Tec2i * VState.vLocation;
196 // Compute the local frame ECEF velocity
197 vVel = Tb2l * VState.vUVW;
199 // Refer to Stevens and Lewis, 1.5-14a, pg. 49.
200 // This is the rotation rate of the "Local" frame, expressed in the local frame.
202 FGColumnVector3 vOmegaLocal = FGColumnVector3(
204 -radInv*vVel(eNorth),
205 -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
207 // Set the angular velocities of the body frame relative to the ECEF frame,
208 // expressed in the body frame. Effectively, this is:
209 // w_b/e = w_b/l + w_l/e
210 VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
212 FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal;
214 VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
216 // Make an initial run and set past values
217 CalculatePQRdot(); // Angular rate derivative
218 CalculateUVWdot(); // Translational rate derivative
219 CalculateQuatdot(); // Angular orientation derivative
220 CalculateInertialVelocity(); // Translational position derivative
222 // Initialize past values deques
223 VState.dqPQRdot.clear();
224 VState.dqUVWdot.clear();
225 VState.dqInertialVelocity.clear();
226 VState.dqQtrndot.clear();
227 for (int i=0; i<4; i++) {
228 VState.dqPQRdot.push_front(vPQRdot);
229 VState.dqUVWdot.push_front(vUVWdot);
230 VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
231 VState.dqQtrndot.push_front(vQtrndot);
234 // Recompute the LocalTerrainRadius.
235 RecomputeLocalTerrainRadius();
238 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
240 Purpose: Called on a schedule to perform EOM integration
241 Notes: [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
242 In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
244 At the top of this Run() function, see several "shortcuts" (or, aliases) being
245 set up for use later, rather than using the longer class->function() notation.
247 This propagation is done using the current state values
248 and current derivatives. Based on these values we compute an approximation to the
249 state values for (now + dt).
251 In the code below, variables named beginning with a small "v" refer to a
252 a column vector, variables named beginning with a "T" refer to a transformation
253 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
258 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 CalculateQuatdot(); // Angular orientation derivative
273 CalculateInertialVelocity(); // Translational position derivative
275 // Propagate rotational / translational velocity, angular /translational position, respectively.
276 Integrate(VState.vPQRi, vPQRdot, VState.dqPQRdot, dt, integrator_rotational_rate);
277 Integrate(VState.vUVW, vUVWdot, VState.dqUVWdot, dt, integrator_translational_rate);
278 Integrate(VState.qAttitudeECI, vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
279 Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
281 VState.qAttitudeECI.Normalize(); // Normalize the ECI Attitude quaternion
283 VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle()); // Update the Earth position angle (EPA)
285 // Update the "Location-based" transformation matrices from the vLocation vector.
287 Ti2ec = GetTi2ec(); // ECI to ECEF transform
288 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
289 Tl2ec = GetTl2ec(); // local to ECEF transform
290 Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
292 Tl2i = Ti2l.Transposed();
294 // Update the "Orientation-based" transformation matrices from the orientation quaternion
296 Ti2b = GetTi2b(); // ECI to body frame transform
297 Tb2i = Ti2b.Transposed(); // body to ECI frame transform
298 Tl2b = Ti2b*Tl2i; // local to body frame transform
299 Tb2l = Tl2b.Transposed(); // body to local frame transform
300 Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
301 Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
303 // Set auxililary state variables
304 VState.vLocation = Ti2ec*VState.vInertialPosition;
305 RecomputeLocalTerrainRadius();
307 // Calculate current aircraft radius from center of planet
308 VehicleRadius = VState.vInertialPosition.Magnitude();
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 if (!GroundReactions->GetWOW() && Aircraft->GetHoldDown() == 0) {
390 vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
393 // Include Gravitation accel
396 vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) );
399 vGravAccel = Tec2b * Inertial->GetGravityJ2(VState.vLocation);
403 vUVWdot += vGravAccel;
406 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
407 // Transform the velocity vector of the body relative to the origin (Earth
408 // center) to be expressed in the inertial frame, and add the vehicle velocity
409 // contribution due to the rotation of the planet.
410 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
411 // Second edition (2004), eqn 1.5-16c (page 50)
413 void FGPropagate::CalculateInertialVelocity(void)
415 VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
418 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
420 void FGPropagate::Integrate( FGColumnVector3& Integrand,
421 FGColumnVector3& Val,
422 deque <FGColumnVector3>& ValDot,
424 eIntegrateType integration_type)
426 ValDot.push_front(Val);
429 switch(integration_type) {
430 case eRectEuler: Integrand += dt*ValDot[0];
432 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
434 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
436 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
438 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
440 case eNone: // do nothing, freeze translational rate
445 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
447 void FGPropagate::Integrate( FGQuaternion& Integrand,
449 deque <FGQuaternion>& ValDot,
451 eIntegrateType integration_type)
453 ValDot.push_front(Val);
456 switch(integration_type) {
457 case eRectEuler: Integrand += dt*ValDot[0];
459 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
461 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
463 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
465 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
467 case eNone: // do nothing, freeze translational rate
472 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
474 void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
475 VState.qAttitudeECI = Qi;
478 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
480 void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
481 VState.vInertialVelocity = Vi;
484 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
486 void FGPropagate::RecomputeLocalTerrainRadius(void)
488 double t = FDMExec->GetSimTime();
490 // Get the LocalTerrain radius.
491 // FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
492 // LocalTerrainRadius = contactloc.GetRadius();
493 LocalTerrainRadius = FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius();
496 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
498 void FGPropagate::SetTerrainElevation(double terrainElev)
500 LocalTerrainRadius = terrainElev + SeaLevelRadius;
501 FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
504 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
506 double FGPropagate::GetTerrainElevation(void) const
508 return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
511 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
512 //Todo: when should this be called - when should the new EPA be used to
513 // calculate the transformation matrix, so that the matrix is not a step
514 // ahead of the sim and the associated calculations?
515 const FGMatrix33& FGPropagate::GetTi2ec(void)
517 return VState.vLocation.GetTi2ec();
520 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
522 const FGMatrix33& FGPropagate::GetTec2i(void)
524 return VState.vLocation.GetTec2i();
527 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
529 void FGPropagate::SetAltitudeASL(double altASL)
531 VState.vLocation.SetRadius( altASL + SeaLevelRadius );
534 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
536 double FGPropagate::GetLocalTerrainRadius(void) const
538 return LocalTerrainRadius;
541 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
543 double FGPropagate::GetDistanceAGL(void) const
545 return VState.vLocation.GetRadius() - LocalTerrainRadius;
548 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
550 void FGPropagate::SetDistanceAGL(double tt)
552 VState.vLocation.SetRadius( tt + LocalTerrainRadius );
555 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
557 void FGPropagate::bind(void)
559 typedef double (FGPropagate::*PMF)(int) const;
561 PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
563 PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
564 PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
565 PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
567 PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
568 PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
569 PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
571 PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
572 PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
573 PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
575 PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
576 PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
577 PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
579 PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
581 PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
582 PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
583 PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
585 PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
586 PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
587 PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
589 PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
590 PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
591 PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
592 PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
593 PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
594 PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
595 PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
596 PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
597 PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
598 PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
599 PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
600 PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
601 &FGPropagate::GetTerrainElevation,
602 &FGPropagate::SetTerrainElevation, false);
604 PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
606 PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
607 PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
608 PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
610 PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
611 PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
612 PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
614 PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
615 PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
616 PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
617 PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
618 PropertyManager->Tie("simulation/gravity-model", &gravType);
621 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
622 // The bitmasked value choices are as follows:
623 // unset: In this case (the default) JSBSim would only print
624 // out the normally expected messages, essentially echoing
625 // the config files as they are read. If the environment
626 // variable is not set, debug_lvl is set to 1 internally
627 // 0: This requests JSBSim not to output any messages
629 // 1: This value explicity requests the normal JSBSim
631 // 2: This value asks for a message to be printed out when
632 // a class is instantiated
633 // 4: When this value is set, a message is displayed when a
634 // FGModel object executes its Run() method
635 // 8: When this value is set, various runtime state variables
636 // are printed out periodically
637 // 16: When set various parameters are sanity checked and
638 // a message is printed out when they go out of bounds
640 void FGPropagate::Debug(int from)
642 if (debug_lvl <= 0) return;
644 if (debug_lvl & 1) { // Standard console startup message output
645 if (from == 0) { // Constructor
649 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
650 if (from == 0) cout << "Instantiated: FGPropagate" << endl;
651 if (from == 1) cout << "Destroyed: FGPropagate" << endl;
653 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
655 if (debug_lvl & 8 && from == 2) { // Runtime state variables
656 cout << endl << fgblue << highint << left
657 << " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
660 cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
661 << Inertial->GetEarthPositionAngleDeg() << endl;
663 cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
664 cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
665 cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
666 cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
667 cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
668 cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
669 cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
670 cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
672 cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
673 << reset << endl << Tec2b.Dump("\t", " ") << endl;
674 cout << highint << " Associated Euler angles (deg): " << setw(8)
675 << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
678 cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
679 << reset << endl << Tb2ec.Dump("\t", " ") << endl;
680 cout << highint << " Associated Euler angles (deg): " << setw(8)
681 << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
684 cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
685 << reset << endl << Tl2b.Dump("\t", " ") << endl;
686 cout << highint << " Associated Euler angles (deg): " << setw(8)
687 << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
690 cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
691 << reset << endl << Tb2l.Dump("\t", " ") << endl;
692 cout << highint << " Associated Euler angles (deg): " << setw(8)
693 << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
696 cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
697 << reset << endl << Tl2ec.Dump("\t", " ") << endl;
698 cout << highint << " Associated Euler angles (deg): " << setw(8)
699 << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
702 cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
703 << reset << endl << Tec2l.Dump("\t", " ") << endl;
704 cout << highint << " Associated Euler angles (deg): " << setw(8)
705 << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
708 cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
709 << reset << endl << Tec2i.Dump("\t", " ") << endl;
710 cout << highint << " Associated Euler angles (deg): " << setw(8)
711 << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
714 cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
715 << reset << endl << Ti2ec.Dump("\t", " ") << endl;
716 cout << highint << " Associated Euler angles (deg): " << setw(8)
717 << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
720 cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
721 << reset << endl << Ti2b.Dump("\t", " ") << endl;
722 cout << highint << " Associated Euler angles (deg): " << setw(8)
723 << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
726 cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
727 << reset << endl << Tb2i.Dump("\t", " ") << endl;
728 cout << highint << " Associated Euler angles (deg): " << setw(8)
729 << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
732 cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
733 << reset << endl << Ti2l.Dump("\t", " ") << endl;
734 cout << highint << " Associated Euler angles (deg): " << setw(8)
735 << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
738 cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
739 << reset << endl << Tl2i.Dump("\t", " ") << endl;
740 cout << highint << " Associated Euler angles (deg): " << setw(8)
741 << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
744 cout << setprecision(6); // reset the output stream
746 if (debug_lvl & 16) { // Sanity checking
747 if (from == 2) { // State sanity checking
748 if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
749 cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
752 if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
753 cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
756 if (fabs(GetDistanceAGL()) > 1e10) {
757 cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
762 if (debug_lvl & 64) {
763 if (from == 0) { // Constructor
764 cout << IdSrc << endl;
765 cout << IdHdr << endl;