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 "input_output/FGPropertyManager.h"
71 static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.96 2011/09/17 15:36:35 bcoconni Exp $";
72 static const char *IdHdr = ID_PROPAGATE;
74 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
76 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
78 FGPropagate::FGPropagate(FGFDMExec* fdmex)
80 LocalTerrainRadius(0),
87 vInertialVelocity.InitMatrix();
89 /// These define the indices use to select the various integrators.
90 // eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
92 integrator_rotational_rate = eRectEuler;
93 integrator_translational_rate = eAdamsBashforth2;
94 integrator_rotational_position = eRectEuler;
95 integrator_translational_position = eAdamsBashforth3;
97 VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
98 VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
99 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
100 VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0));
106 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
108 FGPropagate::~FGPropagate(void)
113 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
115 bool FGPropagate::InitModel(void)
117 // For initialization ONLY:
118 SeaLevelRadius = LocalTerrainRadius = in.RefRadius;
119 FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
121 VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
122 VState.vLocation.SetEllipse(in.SemiMajor, in.SemiMinor);
124 vInertialVelocity.InitMatrix();
126 VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
127 VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
128 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
129 VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
131 integrator_rotational_rate = eRectEuler;
132 integrator_translational_rate = eAdamsBashforth2;
133 integrator_rotational_position = eRectEuler;
134 integrator_translational_position = eAdamsBashforth3;
139 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
141 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
143 SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
145 // Initialize the State Vector elements and the transformation matrices
147 // Set the position lat/lon/radius
148 VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
149 FGIC->GetLatitudeRadIC(),
150 FGIC->GetAltitudeASLFtIC() + SeaLevelRadius);
152 VState.vLocation.SetEarthPositionAngle(0.0);
154 Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
155 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
157 VState.vInertialPosition = Tec2i * VState.vLocation;
159 UpdateLocationMatrices();
161 // Set the orientation from the euler angles (is normalized within the
162 // constructor). The Euler angles represent the orientation of the body
163 // frame relative to the local frame.
164 VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(),
165 FGIC->GetThetaRadIC(),
166 FGIC->GetPsiRadIC() );
168 VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
169 UpdateBodyMatrices();
171 // Set the velocities in the instantaneus body frame
172 VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
173 FGIC->GetVBodyFpsIC(),
174 FGIC->GetWBodyFpsIC() );
176 // Compute the local frame ECEF velocity
177 vVel = Tb2l * VState.vUVW;
179 // Recompute the LocalTerrainRadius.
180 RecomputeLocalTerrainRadius();
182 VehicleRadius = GetRadius();
184 // Set the angular velocities of the body frame relative to the ECEF frame,
185 // expressed in the body frame.
186 VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
188 FGIC->GetRRadpsIC() );
190 VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
192 CalculateInertialVelocity(); // Translational position derivative
195 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
196 // Initialize the past value deques
198 void FGPropagate::InitializeDerivatives()
200 for (int i=0; i<4; i++) {
201 VState.dqPQRidot[i] = in.vPQRidot;
202 VState.dqUVWidot[i] = in.vUVWidot;
203 VState.dqInertialVelocity[i] = VState.vInertialVelocity;
204 VState.dqQtrndot[i] = in.vQtrndot;
208 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
210 Purpose: Called on a schedule to perform EOM integration
211 Notes: [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
212 In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
214 At the top of this Run() function, see several "shortcuts" (or, aliases) being
215 set up for use later, rather than using the longer class->function() notation.
217 This propagation is done using the current state values
218 and current derivatives. Based on these values we compute an approximation to the
219 state values for (now + dt).
221 In the code below, variables named beginning with a small "v" refer to a
222 a column vector, variables named beginning with a "T" refer to a transformation
223 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
228 bool FGPropagate::Run(bool Holding)
230 if (FGModel::Run(Holding)) return true; // Fast return if we have nothing to do ...
231 if (Holding) return false;
233 double dt = in.DeltaT * rate; // The 'stepsize'
237 // Propagate rotational / translational velocity, angular /translational position, respectively.
239 Integrate(VState.vPQRi, in.vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate);
240 Integrate(VState.qAttitudeECI, in.vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
241 Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
242 Integrate(VState.vInertialVelocity, in.vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate);
244 // CAUTION : the order of the operations below is very important to get transformation
245 // matrices that are consistent with the new state of the vehicle
247 // 1. Update the Earth position angle (EPA)
248 VState.vLocation.IncrementEarthPositionAngle(in.vOmegaPlanet(eZ)*(in.DeltaT*rate));
250 // 2. Update the Ti2ec and Tec2i transforms from the updated EPA
251 Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
252 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
254 // 3. Update the location from the updated Ti2ec and inertial position
255 VState.vLocation = Ti2ec*VState.vInertialPosition;
257 // 4. Update the other "Location-based" transformation matrices from the updated
259 UpdateLocationMatrices();
261 // 5. Normalize the ECI Attitude quaternion
262 VState.qAttitudeECI.Normalize();
264 // 6. Update the "Orientation-based" transformation matrices from the updated
265 // orientation quaternion and vLocation vector.
266 UpdateBodyMatrices();
268 // Translational position derivative (velocities are integrated in the inertial frame)
271 // Set auxilliary state variables
272 RecomputeLocalTerrainRadius();
274 VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
276 VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
278 VState.qAttitudeLocal = Tl2b.GetQuaternion();
280 // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
281 vVel = Tb2l * VState.vUVW;
289 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
290 // Transform the velocity vector of the body relative to the origin (Earth
291 // center) to be expressed in the inertial frame, and add the vehicle velocity
292 // contribution due to the rotation of the planet.
293 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
294 // Second edition (2004), eqn 1.5-16c (page 50)
296 void FGPropagate::CalculateInertialVelocity(void)
298 VState.vInertialVelocity = Tb2i * VState.vUVW + (in.vOmegaPlanet * VState.vInertialPosition);
301 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
302 // Transform the velocity vector of the inertial frame to be expressed in the
303 // body frame relative to the origin (Earth center), and substract the vehicle
304 // velocity contribution due to the rotation of the planet.
306 void FGPropagate::CalculateUVW(void)
308 VState.vUVW = Ti2b * (VState.vInertialVelocity - (in.vOmegaPlanet * VState.vInertialPosition));
311 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
313 void FGPropagate::Integrate( FGColumnVector3& Integrand,
314 FGColumnVector3& Val,
315 deque <FGColumnVector3>& ValDot,
317 eIntegrateType integration_type)
319 ValDot.push_front(Val);
322 switch(integration_type) {
323 case eRectEuler: Integrand += dt*ValDot[0];
325 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
327 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
329 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
331 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
333 case eNone: // do nothing, freeze translational rate
338 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
340 void FGPropagate::Integrate( FGQuaternion& Integrand,
342 deque <FGQuaternion>& ValDot,
344 eIntegrateType integration_type)
346 ValDot.push_front(Val);
349 switch(integration_type) {
350 case eRectEuler: Integrand += dt*ValDot[0];
352 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
354 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
356 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
358 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
360 case eNone: // do nothing, freeze rotational rate
365 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
367 void FGPropagate::UpdateLocationMatrices(void)
369 Tl2ec = VState.vLocation.GetTl2ec(); // local to ECEF transform
370 Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
371 Ti2l = VState.vLocation.GetTi2l(); // ECI to local frame transform
372 Tl2i = Ti2l.Transposed(); // local to ECI transform
375 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
377 void FGPropagate::UpdateBodyMatrices(void)
379 Ti2b = VState.qAttitudeECI.GetT(); // ECI to body frame transform
380 Tb2i = Ti2b.Transposed(); // body to ECI frame transform
381 Tl2b = Ti2b * Tl2i; // local to body frame transform
382 Tb2l = Tl2b.Transposed(); // body to local frame transform
383 Tec2b = Ti2b * Tec2i; // ECEF to body frame transform
384 Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
387 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
389 void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
390 VState.qAttitudeECI = Qi;
391 VState.qAttitudeECI.Normalize();
392 UpdateBodyMatrices();
393 VState.qAttitudeLocal = Tl2b.GetQuaternion();
396 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
398 void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
399 VState.vInertialVelocity = Vi;
401 vVel = Tb2l * VState.vUVW;
404 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
406 void FGPropagate::SetInertialRates(FGColumnVector3 vRates) {
407 VState.vPQRi = Ti2b * vRates;
408 VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
411 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
413 void FGPropagate::RecomputeLocalTerrainRadius(void)
415 FGLocation contactloc;
416 FGColumnVector3 dummy;
417 double t = FDMExec->GetSimTime();
419 // Get the LocalTerrain radius.
420 FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc,
421 dummy, LocalTerrainVelocity, LocalTerrainAngularVelocity);
422 LocalTerrainRadius = contactloc.GetRadius();
423 FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
426 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
428 void FGPropagate::SetTerrainElevation(double terrainElev)
430 LocalTerrainRadius = terrainElev + SeaLevelRadius;
431 FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
434 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
436 double FGPropagate::GetTerrainElevation(void) const
438 return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
441 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
443 double FGPropagate::GetDistanceAGL(void) const
445 return VState.vLocation.GetRadius() - LocalTerrainRadius;
448 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
450 void FGPropagate::SetVState(const VehicleState& vstate)
452 //ToDo: Shouldn't all of these be set from the vstate vector passed in?
453 VState.vLocation = vstate.vLocation;
454 VState.vLocation.SetEarthPositionAngle(vstate.vLocation.GetEPA());
455 Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
456 Tec2i = Ti2ec.Transposed();
457 UpdateLocationMatrices();
458 SetInertialOrientation(vstate.qAttitudeECI);
459 RecomputeLocalTerrainRadius();
460 VehicleRadius = GetRadius();
461 VState.vUVW = vstate.vUVW;
462 vVel = Tb2l * VState.vUVW;
463 VState.vPQR = vstate.vPQR;
464 VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
465 VState.vInertialPosition = vstate.vInertialPosition;
468 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
470 void FGPropagate::UpdateVehicleState(void)
472 RecomputeLocalTerrainRadius();
473 VehicleRadius = GetRadius();
474 VState.vInertialPosition = Tec2i * VState.vLocation;
475 UpdateLocationMatrices();
476 UpdateBodyMatrices();
477 vVel = Tb2l * VState.vUVW;
478 VState.qAttitudeLocal = Tl2b.GetQuaternion();
481 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
483 void FGPropagate::SetLocation(const FGLocation& l)
485 VState.vLocation = l;
486 VState.vLocation.SetEarthPositionAngle(l.GetEPA());
487 Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
488 Tec2i = Ti2ec.Transposed();
489 UpdateVehicleState();
492 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
494 void FGPropagate::DumpState(void)
498 << "------------------------------------------------------------------" << reset << endl;
500 << "State Report at sim time: " << FDMExec->GetSimTime() << " seconds" << reset << endl;
501 cout << " " << underon
502 << "Position" << underoff << endl;
503 cout << " ECI: " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl;
504 cout << " ECEF: " << VState.vLocation << " (x,y,z, in ft)" << endl;
505 cout << " Local: " << VState.vLocation.GetLatitudeDeg()
506 << ", " << VState.vLocation.GetLongitudeDeg()
507 << ", " << GetAltitudeASL() << " (lat, lon, alt in deg and ft)" << endl;
509 cout << endl << " " << underon
510 << "Orientation" << underoff << endl;
511 cout << " ECI: " << VState.qAttitudeECI.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
512 cout << " Local: " << VState.qAttitudeLocal.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
514 cout << endl << " " << underon
515 << "Velocity" << underoff << endl;
516 cout << " ECI: " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl;
517 cout << " ECEF: " << (Tb2ec * VState.vUVW).Dump(", ") << " (x,y,z in ft/s)" << endl;
518 cout << " Local: " << GetVel() << " (n,e,d in ft/sec)" << endl;
519 cout << " Body: " << GetUVW() << " (u,v,w in ft/sec)" << endl;
521 cout << endl << " " << underon
522 << "Body Rates (relative to given frame, expressed in body frame)" << underoff << endl;
523 cout << " ECI: " << (VState.vPQRi*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
524 cout << " ECEF: " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
527 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
529 void FGPropagate::bind(void)
531 typedef double (FGPropagate::*PMF)(int) const;
533 PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
535 PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
536 PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
537 PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
539 PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
540 PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
541 PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
543 PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
544 PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
545 PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
547 PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
548 PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
549 PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
551 PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
553 PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
554 PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
555 PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude, false);
556 PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude, false);
557 PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg, false);
558 PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg, false);
559 PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
560 PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
561 PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
562 PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
563 PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
564 PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
565 &FGPropagate::GetTerrainElevation,
566 &FGPropagate::SetTerrainElevation, false);
568 PropertyManager->Tie("position/epa-rad", this, &FGPropagate::GetEarthPositionAngle);
569 PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
571 PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
572 PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
573 PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
575 PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
576 PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
577 PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
579 PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
580 PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
581 PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
582 PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
585 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
586 // The bitmasked value choices are as follows:
587 // unset: In this case (the default) JSBSim would only print
588 // out the normally expected messages, essentially echoing
589 // the config files as they are read. If the environment
590 // variable is not set, debug_lvl is set to 1 internally
591 // 0: This requests JSBSim not to output any messages
593 // 1: This value explicity requests the normal JSBSim
595 // 2: This value asks for a message to be printed out when
596 // a class is instantiated
597 // 4: When this value is set, a message is displayed when a
598 // FGModel object executes its Run() method
599 // 8: When this value is set, various runtime state variables
600 // are printed out periodically
601 // 16: When set various parameters are sanity checked and
602 // a message is printed out when they go out of bounds
604 void FGPropagate::Debug(int from)
606 if (debug_lvl <= 0) return;
608 if (debug_lvl & 1) { // Standard console startup message output
609 if (from == 0) { // Constructor
613 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
614 if (from == 0) cout << "Instantiated: FGPropagate" << endl;
615 if (from == 1) cout << "Destroyed: FGPropagate" << endl;
617 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
619 if (debug_lvl & 8 && from == 2) { // Runtime state variables
620 cout << endl << fgblue << highint << left
621 << " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
624 cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
625 << GetEarthPositionAngleDeg() << endl;
627 cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
628 cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
629 cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
630 cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
631 cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
632 cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
633 cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
634 // cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
636 cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
637 << reset << endl << Tec2b.Dump("\t", " ") << endl;
638 cout << highint << " Associated Euler angles (deg): " << setw(8)
639 << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
642 cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
643 << reset << endl << Tb2ec.Dump("\t", " ") << endl;
644 cout << highint << " Associated Euler angles (deg): " << setw(8)
645 << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
648 cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
649 << reset << endl << Tl2b.Dump("\t", " ") << endl;
650 cout << highint << " Associated Euler angles (deg): " << setw(8)
651 << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
654 cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
655 << reset << endl << Tb2l.Dump("\t", " ") << endl;
656 cout << highint << " Associated Euler angles (deg): " << setw(8)
657 << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
660 cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
661 << reset << endl << Tl2ec.Dump("\t", " ") << endl;
662 cout << highint << " Associated Euler angles (deg): " << setw(8)
663 << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
666 cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
667 << reset << endl << Tec2l.Dump("\t", " ") << endl;
668 cout << highint << " Associated Euler angles (deg): " << setw(8)
669 << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
672 cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
673 << reset << endl << Tec2i.Dump("\t", " ") << endl;
674 cout << highint << " Associated Euler angles (deg): " << setw(8)
675 << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
678 cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
679 << reset << endl << Ti2ec.Dump("\t", " ") << endl;
680 cout << highint << " Associated Euler angles (deg): " << setw(8)
681 << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
684 cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
685 << reset << endl << Ti2b.Dump("\t", " ") << endl;
686 cout << highint << " Associated Euler angles (deg): " << setw(8)
687 << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
690 cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
691 << reset << endl << Tb2i.Dump("\t", " ") << endl;
692 cout << highint << " Associated Euler angles (deg): " << setw(8)
693 << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
696 cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
697 << reset << endl << Ti2l.Dump("\t", " ") << endl;
698 cout << highint << " Associated Euler angles (deg): " << setw(8)
699 << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
702 cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
703 << reset << endl << Tl2i.Dump("\t", " ") << endl;
704 cout << highint << " Associated Euler angles (deg): " << setw(8)
705 << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
708 cout << setprecision(6); // reset the output stream
710 if (debug_lvl & 16) { // Sanity checking
711 if (from == 2) { // State sanity checking
712 if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
713 cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
716 if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
717 cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
720 if (fabs(GetDistanceAGL()) > 1e10) {
721 cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
726 if (debug_lvl & 64) {
727 if (from == 0) { // Constructor
728 cout << IdSrc << endl;
729 cout << IdHdr << endl;