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.100 2011/11/06 18:14:51 bcoconni Exp $";
72 static const char *IdHdr = ID_PROPAGATE;
74 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
76 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
78 FGPropagate::FGPropagate(FGFDMExec* fdmex)
85 vInertialVelocity.InitMatrix();
87 /// These define the indices use to select the various integrators.
88 // eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
90 integrator_rotational_rate = eRectEuler;
91 integrator_translational_rate = eAdamsBashforth2;
92 integrator_rotational_position = eRectEuler;
93 integrator_translational_position = eAdamsBashforth3;
95 VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
96 VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
97 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
98 VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0));
104 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
106 FGPropagate::~FGPropagate(void)
111 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
113 bool FGPropagate::InitModel(void)
115 // For initialization ONLY:
116 VState.vLocation.SetEllipse(in.SemiMajor, in.SemiMinor);
117 VState.vLocation.SetAltitudeAGL(4.0, FDMExec->GetSimTime());
119 vInertialVelocity.InitMatrix();
121 VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
122 VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0));
123 VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
124 VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
126 integrator_rotational_rate = eRectEuler;
127 integrator_translational_rate = eAdamsBashforth2;
128 integrator_rotational_position = eRectEuler;
129 integrator_translational_position = eAdamsBashforth3;
134 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
136 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
138 // Initialize the State Vector elements and the transformation matrices
140 // Set the position lat/lon/radius
141 VState.vLocation = FGIC->GetPosition();
143 Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
144 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
146 VState.vInertialPosition = Tec2i * VState.vLocation;
148 UpdateLocationMatrices();
150 // Set the orientation from the euler angles (is normalized within the
151 // constructor). The Euler angles represent the orientation of the body
152 // frame relative to the local frame.
153 VState.qAttitudeLocal = FGIC->GetOrientation();
155 VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
156 UpdateBodyMatrices();
158 // Set the velocities in the instantaneus body frame
159 VState.vUVW = FGIC->GetUVWFpsIC();
161 // Compute the local frame ECEF velocity
162 vVel = Tb2l * VState.vUVW;
164 // Compute local terrain velocity
165 RecomputeLocalTerrainVelocity();
166 VehicleRadius = GetRadius();
168 // Set the angular velocities of the body frame relative to the ECEF frame,
169 // expressed in the body frame.
170 VState.vPQR = FGIC->GetPQRRadpsIC();
172 VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
174 CalculateInertialVelocity(); // Translational position derivative
177 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
178 // Initialize the past value deques
180 void FGPropagate::InitializeDerivatives()
182 for (int i=0; i<4; i++) {
183 VState.dqPQRidot[i] = in.vPQRidot;
184 VState.dqUVWidot[i] = in.vUVWidot;
185 VState.dqInertialVelocity[i] = VState.vInertialVelocity;
186 VState.dqQtrndot[i] = in.vQtrndot;
190 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
192 Purpose: Called on a schedule to perform EOM integration
193 Notes: [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
194 In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
196 At the top of this Run() function, see several "shortcuts" (or, aliases) being
197 set up for use later, rather than using the longer class->function() notation.
199 This propagation is done using the current state values
200 and current derivatives. Based on these values we compute an approximation to the
201 state values for (now + dt).
203 In the code below, variables named beginning with a small "v" refer to a
204 a column vector, variables named beginning with a "T" refer to a transformation
205 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
210 bool FGPropagate::Run(bool Holding)
212 if (FGModel::Run(Holding)) return true; // Fast return if we have nothing to do ...
213 if (Holding) return false;
215 double dt = in.DeltaT * rate; // The 'stepsize'
219 // Propagate rotational / translational velocity, angular /translational position, respectively.
221 Integrate(VState.vPQRi, in.vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate);
222 Integrate(VState.qAttitudeECI, in.vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
223 Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
224 Integrate(VState.vInertialVelocity, in.vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate);
226 // CAUTION : the order of the operations below is very important to get transformation
227 // matrices that are consistent with the new state of the vehicle
229 // 1. Update the Earth position angle (EPA)
230 VState.vLocation.IncrementEarthPositionAngle(in.vOmegaPlanet(eZ)*(in.DeltaT*rate));
232 // 2. Update the Ti2ec and Tec2i transforms from the updated EPA
233 Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
234 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
236 // 3. Update the location from the updated Ti2ec and inertial position
237 VState.vLocation = Ti2ec*VState.vInertialPosition;
239 // 4. Update the other "Location-based" transformation matrices from the updated
241 UpdateLocationMatrices();
243 // 5. Normalize the ECI Attitude quaternion
244 VState.qAttitudeECI.Normalize();
246 // 6. Update the "Orientation-based" transformation matrices from the updated
247 // orientation quaternion and vLocation vector.
248 UpdateBodyMatrices();
250 // Translational position derivative (velocities are integrated in the inertial frame)
253 // Set auxilliary state variables
254 RecomputeLocalTerrainVelocity();
255 VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
257 VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
259 VState.qAttitudeLocal = Tl2b.GetQuaternion();
261 // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
262 vVel = Tb2l * VState.vUVW;
270 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
271 // Transform the velocity vector of the body relative to the origin (Earth
272 // center) to be expressed in the inertial frame, and add the vehicle velocity
273 // contribution due to the rotation of the planet.
274 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
275 // Second edition (2004), eqn 1.5-16c (page 50)
277 void FGPropagate::CalculateInertialVelocity(void)
279 VState.vInertialVelocity = Tb2i * VState.vUVW + (in.vOmegaPlanet * VState.vInertialPosition);
282 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
283 // Transform the velocity vector of the inertial frame to be expressed in the
284 // body frame relative to the origin (Earth center), and substract the vehicle
285 // velocity contribution due to the rotation of the planet.
287 void FGPropagate::CalculateUVW(void)
289 VState.vUVW = Ti2b * (VState.vInertialVelocity - (in.vOmegaPlanet * VState.vInertialPosition));
292 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
294 void FGPropagate::Integrate( FGColumnVector3& Integrand,
295 FGColumnVector3& Val,
296 deque <FGColumnVector3>& ValDot,
298 eIntegrateType integration_type)
300 ValDot.push_front(Val);
303 switch(integration_type) {
304 case eRectEuler: Integrand += dt*ValDot[0];
306 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
308 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
310 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
312 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
314 case eNone: // do nothing, freeze translational rate
319 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
321 void FGPropagate::Integrate( FGQuaternion& Integrand,
323 deque <FGQuaternion>& ValDot,
325 eIntegrateType integration_type)
327 ValDot.push_front(Val);
330 switch(integration_type) {
331 case eRectEuler: Integrand += dt*ValDot[0];
333 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
335 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
337 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
339 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
341 case eNone: // do nothing, freeze rotational rate
346 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
348 void FGPropagate::UpdateLocationMatrices(void)
350 Tl2ec = VState.vLocation.GetTl2ec(); // local to ECEF transform
351 Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
352 Ti2l = VState.vLocation.GetTi2l(); // ECI to local frame transform
353 Tl2i = Ti2l.Transposed(); // local to ECI transform
356 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
358 void FGPropagate::UpdateBodyMatrices(void)
360 Ti2b = VState.qAttitudeECI.GetT(); // ECI to body frame transform
361 Tb2i = Ti2b.Transposed(); // body to ECI frame transform
362 Tl2b = Ti2b * Tl2i; // local to body frame transform
363 Tb2l = Tl2b.Transposed(); // body to local frame transform
364 Tec2b = Ti2b * Tec2i; // ECEF to body frame transform
365 Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
368 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
370 void FGPropagate::SetInertialOrientation(const FGQuaternion& Qi) {
371 VState.qAttitudeECI = Qi;
372 VState.qAttitudeECI.Normalize();
373 UpdateBodyMatrices();
374 VState.qAttitudeLocal = Tl2b.GetQuaternion();
377 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
379 void FGPropagate::SetInertialVelocity(const FGColumnVector3& Vi) {
380 VState.vInertialVelocity = Vi;
382 vVel = Tb2l * VState.vUVW;
385 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
387 void FGPropagate::SetInertialRates(const FGColumnVector3& vRates) {
388 VState.vPQRi = Ti2b * vRates;
389 VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
392 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
394 void FGPropagate::RecomputeLocalTerrainVelocity()
397 FGColumnVector3 normal;
398 VState.vLocation.GetContactPoint(FDMExec->GetSimTime(), contact, normal,
399 LocalTerrainVelocity, LocalTerrainAngularVelocity);
402 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
404 void FGPropagate::SetTerrainElevation(double terrainElev)
406 double radius = terrainElev + VState.vLocation.GetSeaLevelRadius();
407 FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(radius);
411 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
413 void FGPropagate::SetSeaLevelRadius(double tt)
415 FDMExec->GetGroundCallback()->SetSeaLevelRadius(tt);
418 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
420 double FGPropagate::GetLocalTerrainRadius(void) const
422 return VState.vLocation.GetTerrainRadius(FDMExec->GetSimTime());
425 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
427 double FGPropagate::GetDistanceAGL(void) const
429 return VState.vLocation.GetAltitudeAGL(FDMExec->GetSimTime());
432 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
434 void FGPropagate::SetDistanceAGL(double tt)
436 VState.vLocation.SetAltitudeAGL(tt, FDMExec->GetSimTime());
437 UpdateVehicleState();
440 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
442 void FGPropagate::SetVState(const VehicleState& vstate)
444 //ToDo: Shouldn't all of these be set from the vstate vector passed in?
445 VState.vLocation = vstate.vLocation;
446 VState.vLocation.SetEarthPositionAngle(vstate.vLocation.GetEPA());
447 Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
448 Tec2i = Ti2ec.Transposed();
449 UpdateLocationMatrices();
450 SetInertialOrientation(vstate.qAttitudeECI);
451 RecomputeLocalTerrainVelocity();
452 VehicleRadius = GetRadius();
453 VState.vUVW = vstate.vUVW;
454 vVel = Tb2l * VState.vUVW;
455 VState.vPQR = vstate.vPQR;
456 VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
457 VState.vInertialPosition = vstate.vInertialPosition;
460 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
462 void FGPropagate::UpdateVehicleState(void)
464 RecomputeLocalTerrainVelocity();
465 VehicleRadius = GetRadius();
466 VState.vInertialPosition = Tec2i * VState.vLocation;
467 UpdateLocationMatrices();
468 UpdateBodyMatrices();
469 vVel = Tb2l * VState.vUVW;
470 VState.qAttitudeLocal = Tl2b.GetQuaternion();
473 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
475 void FGPropagate::SetLocation(const FGLocation& l)
477 VState.vLocation = l;
478 Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
479 Tec2i = Ti2ec.Transposed();
480 UpdateVehicleState();
483 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
485 void FGPropagate::DumpState(void)
489 << "------------------------------------------------------------------" << reset << endl;
491 << "State Report at sim time: " << FDMExec->GetSimTime() << " seconds" << reset << endl;
492 cout << " " << underon
493 << "Position" << underoff << endl;
494 cout << " ECI: " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl;
495 cout << " ECEF: " << VState.vLocation << " (x,y,z, in ft)" << endl;
496 cout << " Local: " << VState.vLocation.GetLatitudeDeg()
497 << ", " << VState.vLocation.GetLongitudeDeg()
498 << ", " << GetAltitudeASL() << " (lat, lon, alt in deg and ft)" << endl;
500 cout << endl << " " << underon
501 << "Orientation" << underoff << endl;
502 cout << " ECI: " << VState.qAttitudeECI.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
503 cout << " Local: " << VState.qAttitudeLocal.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
505 cout << endl << " " << underon
506 << "Velocity" << underoff << endl;
507 cout << " ECI: " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl;
508 cout << " ECEF: " << (Tb2ec * VState.vUVW).Dump(", ") << " (x,y,z in ft/s)" << endl;
509 cout << " Local: " << GetVel() << " (n,e,d in ft/sec)" << endl;
510 cout << " Body: " << GetUVW() << " (u,v,w in ft/sec)" << endl;
512 cout << endl << " " << underon
513 << "Body Rates (relative to given frame, expressed in body frame)" << underoff << endl;
514 cout << " ECI: " << (VState.vPQRi*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
515 cout << " ECEF: " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
518 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
520 void FGPropagate::bind(void)
522 typedef double (FGPropagate::*PMF)(int) const;
524 PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
526 PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
527 PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
528 PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
530 PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
531 PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
532 PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
534 PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
535 PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
536 PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
538 PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
539 PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
540 PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
542 PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
544 PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
545 PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
546 PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude, false);
547 PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude, false);
548 PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg, false);
549 PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg, false);
550 PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
551 PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
552 PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
553 PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
554 PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
555 PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
556 &FGPropagate::GetTerrainElevation,
557 &FGPropagate::SetTerrainElevation, false);
559 PropertyManager->Tie("position/epa-rad", this, &FGPropagate::GetEarthPositionAngle);
560 PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
562 PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
563 PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
564 PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
566 PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
567 PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
568 PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
570 PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
571 PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
572 PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
573 PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
576 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
577 // The bitmasked value choices are as follows:
578 // unset: In this case (the default) JSBSim would only print
579 // out the normally expected messages, essentially echoing
580 // the config files as they are read. If the environment
581 // variable is not set, debug_lvl is set to 1 internally
582 // 0: This requests JSBSim not to output any messages
584 // 1: This value explicity requests the normal JSBSim
586 // 2: This value asks for a message to be printed out when
587 // a class is instantiated
588 // 4: When this value is set, a message is displayed when a
589 // FGModel object executes its Run() method
590 // 8: When this value is set, various runtime state variables
591 // are printed out periodically
592 // 16: When set various parameters are sanity checked and
593 // a message is printed out when they go out of bounds
595 void FGPropagate::Debug(int from)
597 if (debug_lvl <= 0) return;
599 if (debug_lvl & 1) { // Standard console startup message output
600 if (from == 0) { // Constructor
604 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
605 if (from == 0) cout << "Instantiated: FGPropagate" << endl;
606 if (from == 1) cout << "Destroyed: FGPropagate" << endl;
608 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
610 if (debug_lvl & 8 && from == 2) { // Runtime state variables
611 cout << endl << fgblue << highint << left
612 << " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
615 cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
616 << GetEarthPositionAngleDeg() << endl;
618 cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
619 cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
620 cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
621 cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
622 cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
623 cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
624 cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
625 // cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
627 cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
628 << reset << endl << Tec2b.Dump("\t", " ") << endl;
629 cout << highint << " Associated Euler angles (deg): " << setw(8)
630 << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
633 cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
634 << reset << endl << Tb2ec.Dump("\t", " ") << endl;
635 cout << highint << " Associated Euler angles (deg): " << setw(8)
636 << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
639 cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
640 << reset << endl << Tl2b.Dump("\t", " ") << endl;
641 cout << highint << " Associated Euler angles (deg): " << setw(8)
642 << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
645 cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
646 << reset << endl << Tb2l.Dump("\t", " ") << endl;
647 cout << highint << " Associated Euler angles (deg): " << setw(8)
648 << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
651 cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
652 << reset << endl << Tl2ec.Dump("\t", " ") << endl;
653 cout << highint << " Associated Euler angles (deg): " << setw(8)
654 << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
657 cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
658 << reset << endl << Tec2l.Dump("\t", " ") << endl;
659 cout << highint << " Associated Euler angles (deg): " << setw(8)
660 << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
663 cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
664 << reset << endl << Tec2i.Dump("\t", " ") << endl;
665 cout << highint << " Associated Euler angles (deg): " << setw(8)
666 << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
669 cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
670 << reset << endl << Ti2ec.Dump("\t", " ") << endl;
671 cout << highint << " Associated Euler angles (deg): " << setw(8)
672 << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
675 cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
676 << reset << endl << Ti2b.Dump("\t", " ") << endl;
677 cout << highint << " Associated Euler angles (deg): " << setw(8)
678 << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
681 cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
682 << reset << endl << Tb2i.Dump("\t", " ") << endl;
683 cout << highint << " Associated Euler angles (deg): " << setw(8)
684 << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
687 cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
688 << reset << endl << Ti2l.Dump("\t", " ") << endl;
689 cout << highint << " Associated Euler angles (deg): " << setw(8)
690 << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
693 cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
694 << reset << endl << Tl2i.Dump("\t", " ") << endl;
695 cout << highint << " Associated Euler angles (deg): " << setw(8)
696 << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
699 cout << setprecision(6); // reset the output stream
701 if (debug_lvl & 16) { // Sanity checking
702 if (from == 2) { // State sanity checking
703 if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
704 cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
707 if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
708 cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
711 if (fabs(GetDistanceAGL()) > 1e10) {
712 cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
717 if (debug_lvl & 64) {
718 if (from == 0) { // Constructor
719 cout << IdSrc << endl;
720 cout << IdHdr << endl;