1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2
3 Module: FGPropagate.cpp
4 Author: Jon S. Berndt
5 Date started: 01/05/99
6 Purpose: Integrate the EOM to determine instantaneous position
7 Called by: FGFDMExec
8
9 ------------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) -------------
10
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
13 Software Foundation; either version 2 of the License, or (at your option) any
14 later version.
15
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
19 details.
20
21 You should have received a copy of the GNU Lesser General Public License along
22 with this program; if not, write to the Free Software Foundation, Inc., 59
23 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
24
25 Further information about the GNU Lesser General Public License can also be
26 found on the world wide web at http://www.gnu.org.
27
28 FUNCTIONAL DESCRIPTION
29 --------------------------------------------------------------------------------
30 This class encapsulates the integration of rates and accelerations to get the
31 current position of the aircraft.
32
33 HISTORY
34 --------------------------------------------------------------------------------
35 01/05/99 JSB Created
36
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
42 School, January 1994
43 [2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
44 JSC 12960, July 1977
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,
53 San Diego, 1999
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,
57 December 1973
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,
60 July-August 2001
61
62 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
63 INCLUDES
64 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
65
66 #include <iomanip>
67
68 #include "FGPropagate.h"
69 #include "initialization/FGInitialCondition.h"
70 #include "FGFDMExec.h"
71 #include "simgear/io/iostreams/sgstream.hxx"
72
73 using namespace std;
74
75 namespace JSBSim {
76
77 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
78 CLASS IMPLEMENTATION
79 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
80
FGPropagate(FGFDMExec * fdmex)81 FGPropagate::FGPropagate(FGFDMExec* fdmex)
82 : FGModel(fdmex)
83 {
84 Debug(0);
85 Name = "FGPropagate";
86
87 /// These define the indices use to select the various integrators.
88 // eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
89
90 integrator_rotational_rate = eRectEuler;
91 integrator_translational_rate = eAdamsBashforth2;
92 integrator_rotational_position = eRectEuler;
93 integrator_translational_position = eAdamsBashforth3;
94
95 VState.dqPQRidot.resize(5, FGColumnVector3(0.0,0.0,0.0));
96 VState.dqUVWidot.resize(5, FGColumnVector3(0.0,0.0,0.0));
97 VState.dqInertialVelocity.resize(5, FGColumnVector3(0.0,0.0,0.0));
98 VState.dqQtrndot.resize(5, FGQuaternion(0.0,0.0,0.0));
99
100 epa = 0.0;
101
102 bind();
103 Debug(0);
104 }
105
106 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
107
~FGPropagate(void)108 FGPropagate::~FGPropagate(void)
109 {
110 Debug(1);
111 }
112
113 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
114
InitModel(void)115 bool FGPropagate::InitModel(void)
116 {
117 if (!FGModel::InitModel()) return false;
118
119 // For initialization ONLY:
120 VState.vLocation.SetEllipse(in.SemiMajor, in.SemiMinor);
121 VState.vLocation.SetAltitudeAGL(4.0);
122
123 VState.dqPQRidot.resize(5, FGColumnVector3(0.0,0.0,0.0));
124 VState.dqUVWidot.resize(5, FGColumnVector3(0.0,0.0,0.0));
125 VState.dqInertialVelocity.resize(5, FGColumnVector3(0.0,0.0,0.0));
126 VState.dqQtrndot.resize(5, FGColumnVector3(0.0,0.0,0.0));
127
128 integrator_rotational_rate = eRectEuler;
129 integrator_translational_rate = eAdamsBashforth2;
130 integrator_rotational_position = eRectEuler;
131 integrator_translational_position = eAdamsBashforth3;
132
133 epa = 0.0;
134
135 return true;
136 }
137
138 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
139
SetInitialState(const FGInitialCondition * FGIC)140 void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
141 {
142 // Initialize the State Vector elements and the transformation matrices
143
144 // Set the position lat/lon/radius
145 VState.vLocation = FGIC->GetPosition();
146
147 epa = FGIC->GetEarthPositionAngleIC();
148 Ti2ec = FGMatrix33(cos(epa), sin(epa), 0.0,
149 -sin(epa), cos(epa), 0.0,
150 0.0, 0.0, 1.0);
151 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
152
153 VState.vInertialPosition = Tec2i * VState.vLocation;
154
155 UpdateLocationMatrices();
156
157 // Set the orientation from the euler angles (is normalized within the
158 // constructor). The Euler angles represent the orientation of the body
159 // frame relative to the local frame.
160 VState.qAttitudeLocal = FGIC->GetOrientation();
161
162 VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
163 UpdateBodyMatrices();
164
165 // Set the velocities in the instantaneus body frame
166 VState.vUVW = FGIC->GetUVWFpsIC();
167
168 // Compute the local frame ECEF velocity
169 vVel = Tb2l * VState.vUVW;
170
171 // Compute local terrain velocity
172 RecomputeLocalTerrainVelocity();
173
174 // Set the angular velocities of the body frame relative to the ECEF frame,
175 // expressed in the body frame.
176 VState.vPQR = FGIC->GetPQRRadpsIC();
177
178 VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
179
180 CalculateInertialVelocity(); // Translational position derivative
181 CalculateQuatdot(); // Angular orientation derivative
182 }
183
184 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
185 // Initialize the past value deques
186
InitializeDerivatives()187 void FGPropagate::InitializeDerivatives()
188 {
189 VState.dqPQRidot.assign(5, in.vPQRidot);
190 VState.dqUVWidot.assign(5, in.vUVWidot);
191 VState.dqInertialVelocity.assign(5, VState.vInertialVelocity);
192 VState.dqQtrndot.assign(5, VState.vQtrndot);
193 }
194
195 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
196 /*
197 Purpose: Called on a schedule to perform EOM integration
198 Notes: [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
199 In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
200
201 At the top of this Run() function, see several "shortcuts" (or, aliases) being
202 set up for use later, rather than using the longer class->function() notation.
203
204 This propagation is done using the current state values
205 and current derivatives. Based on these values we compute an approximation to the
206 state values for (now + dt).
207
208 In the code below, variables named beginning with a small "v" refer to a
209 a column vector, variables named beginning with a "T" refer to a transformation
210 matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
211 Inertial.
212
213 */
214
Run(bool Holding)215 bool FGPropagate::Run(bool Holding)
216 {
217 if (FGModel::Run(Holding)) return true; // Fast return if we have nothing to do ...
218 if (Holding) return false;
219
220 double dt = in.DeltaT * rate; // The 'stepsize'
221
222 // Propagate rotational / translational velocity, angular /translational position, respectively.
223
224 if (!FDMExec->IntegrationSuspended()) {
225 Integrate(VState.qAttitudeECI, VState.vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
226 Integrate(VState.vPQRi, in.vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate);
227 Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
228 Integrate(VState.vInertialVelocity, in.vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate);
229 }
230
231 // CAUTION : the order of the operations below is very important to get transformation
232 // matrices that are consistent with the new state of the vehicle
233
234 // 1. Update the Earth position angle (EPA)
235 epa += in.vOmegaPlanet(eZ)*dt;
236
237 // 2. Update the Ti2ec and Tec2i transforms from the updated EPA
238 double cos_epa = cos(epa);
239 double sin_epa = sin(epa);
240 Ti2ec = FGMatrix33(cos_epa, sin_epa, 0.0,
241 -sin_epa, cos_epa, 0.0,
242 0.0, 0.0, 1.0);
243 Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
244
245 // 3. Update the location from the updated Ti2ec and inertial position
246 VState.vLocation = Ti2ec*VState.vInertialPosition;
247
248 // 4. Update the other "Location-based" transformation matrices from the updated
249 // vLocation vector.
250 UpdateLocationMatrices();
251
252 // 5. Update the "Orientation-based" transformation matrices from the updated
253 // orientation quaternion and vLocation vector.
254 UpdateBodyMatrices();
255
256 // Translational position derivative (velocities are integrated in the inertial frame)
257 CalculateUVW();
258
259 // Set auxilliary state variables
260 RecomputeLocalTerrainVelocity();
261
262 VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
263
264 // Angular orientation derivative
265 CalculateQuatdot();
266
267 VState.qAttitudeLocal = Tl2b.GetQuaternion();
268
269 // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
270 vVel = Tb2l * VState.vUVW;
271
272 Debug(2);
273 return false;
274 }
275
276 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
277
SetHoldDown(bool hd)278 void FGPropagate::SetHoldDown(bool hd)
279 {
280 if (hd) {
281 VState.vUVW.InitMatrix();
282 CalculateInertialVelocity();
283 VState.vPQR.InitMatrix();
284 VState.vPQRi = Ti2b * in.vOmegaPlanet;
285 CalculateQuatdot();
286 InitializeDerivatives();
287 }
288 }
289
290 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
291 // Compute the quaternion orientation derivative
292 //
293 // vQtrndot is the quaternion derivative.
294 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
295 // Second edition (2004), eqn 1.5-16b (page 50)
296
CalculateQuatdot(void)297 void FGPropagate::CalculateQuatdot(void)
298 {
299 // Compute quaternion orientation derivative on current body rates
300 VState.vQtrndot = VState.qAttitudeECI.GetQDot(VState.vPQRi);
301 }
302
303 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
304 // Transform the velocity vector of the body relative to the origin (Earth
305 // center) to be expressed in the inertial frame, and add the vehicle velocity
306 // contribution due to the rotation of the planet.
307 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
308 // Second edition (2004), eqn 1.5-16c (page 50)
309
CalculateInertialVelocity(void)310 void FGPropagate::CalculateInertialVelocity(void)
311 {
312 VState.vInertialVelocity = Tb2i * VState.vUVW + (in.vOmegaPlanet * VState.vInertialPosition);
313 }
314
315 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
316 // Transform the velocity vector of the inertial frame to be expressed in the
317 // body frame relative to the origin (Earth center), and substract the vehicle
318 // velocity contribution due to the rotation of the planet.
319
CalculateUVW(void)320 void FGPropagate::CalculateUVW(void)
321 {
322 VState.vUVW = Ti2b * (VState.vInertialVelocity - (in.vOmegaPlanet * VState.vInertialPosition));
323 }
324
325 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
326
Integrate(FGColumnVector3 & Integrand,FGColumnVector3 & Val,deque<FGColumnVector3> & ValDot,double dt,eIntegrateType integration_type)327 void FGPropagate::Integrate( FGColumnVector3& Integrand,
328 FGColumnVector3& Val,
329 deque <FGColumnVector3>& ValDot,
330 double dt,
331 eIntegrateType integration_type)
332 {
333 ValDot.push_front(Val);
334 ValDot.pop_back();
335
336 switch(integration_type) {
337 case eRectEuler: Integrand += dt*ValDot[0];
338 break;
339 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
340 break;
341 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
342 break;
343 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
344 break;
345 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
346 break;
347 case eAdamsBashforth5: Integrand += dt*((1901./720.)*ValDot[0] - (1387./360.)*ValDot[1] + (109./30.)*ValDot[2] - (637./360.)*ValDot[3] + (251./720.)*ValDot[4]);
348 break;
349 case eNone: // do nothing, freeze translational rate
350 break;
351 case eBuss1:
352 case eBuss2:
353 case eLocalLinearization:
354 throw("Can only use Buss (1 & 2) or local linearization integration methods in for rotational position!");
355 default:
356 break;
357 }
358 }
359
360 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
361
Integrate(FGQuaternion & Integrand,FGQuaternion & Val,deque<FGQuaternion> & ValDot,double dt,eIntegrateType integration_type)362 void FGPropagate::Integrate( FGQuaternion& Integrand,
363 FGQuaternion& Val,
364 deque <FGQuaternion>& ValDot,
365 double dt,
366 eIntegrateType integration_type)
367 {
368 ValDot.push_front(Val);
369 ValDot.pop_back();
370
371 switch(integration_type) {
372 case eRectEuler: Integrand += dt*ValDot[0];
373 break;
374 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
375 break;
376 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
377 break;
378 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
379 break;
380 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
381 break;
382 case eAdamsBashforth5: Integrand += dt*((1901./720.)*ValDot[0] - (1387./360.)*ValDot[1] + (109./30.)*ValDot[2] - (637./360.)*ValDot[3] + (251./720.)*ValDot[4]);
383 break;
384 case eBuss1:
385 {
386 // This is the first order method as described in Samuel R. Buss paper[6].
387 // The formula from Buss' paper is transposed below to quaternions and is
388 // actually the exact solution of the quaternion differential equation
389 // qdot = 1/2*w*q when w is constant.
390 Integrand = Integrand * QExp(0.5 * dt * VState.vPQRi);
391 }
392 return; // No need to normalize since the quaternion exponential is always normal
393 case eBuss2:
394 {
395 // This is the 'augmented second-order method' from S.R. Buss paper [6].
396 // Unlike Runge-Kutta or Adams-Bashforth, it is a one-pass second-order
397 // method (see reference [6]).
398 FGColumnVector3 wi = VState.vPQRi;
399 FGColumnVector3 wdoti = in.vPQRidot;
400 FGColumnVector3 omega = wi + 0.5*dt*wdoti + dt*dt/12.*wdoti*wi;
401 Integrand = Integrand * QExp(0.5 * dt * omega);
402 }
403 return; // No need to normalize since the quaternion exponential is always normal
404 case eLocalLinearization:
405 {
406 // This is the local linearization algorithm of Barker et al. (see ref. [7])
407 // It is also a one-pass second-order method. The code below is based on the
408 // more compact formulation issued from equation (107) of ref. [8]. The
409 // constants C1, C2, C3 and C4 have the same value than those in ref. [7] pp. 11
410 FGColumnVector3 wi = 0.5 * VState.vPQRi;
411 FGColumnVector3 wdoti = 0.5 * in.vPQRidot;
412 double omegak2 = DotProduct(VState.vPQRi, VState.vPQRi);
413 double omegak = omegak2 > 1E-6 ? sqrt(omegak2) : 1E-6;
414 double rhok = 0.5 * dt * omegak;
415 double C1 = cos(rhok);
416 double C2 = 2.0 * sin(rhok) / omegak;
417 double C3 = 4.0 * (1.0 - C1) / (omegak*omegak);
418 double C4 = 4.0 * (dt - C2) / (omegak*omegak);
419 FGColumnVector3 Omega = C2*wi + C3*wdoti + C4*wi*wdoti;
420 FGQuaternion q;
421
422 q(1) = C1 - C4*DotProduct(wi, wdoti);
423 q(2) = Omega(eP);
424 q(3) = Omega(eQ);
425 q(4) = Omega(eR);
426
427 Integrand = Integrand * q;
428
429 /* Cross check with ref. [7] pp.11-12 formulas and code pp. 20
430 double pk = VState.vPQRi(eP);
431 double qk = VState.vPQRi(eQ);
432 double rk = VState.vPQRi(eR);
433 double pdotk = in.vPQRidot(eP);
434 double qdotk = in.vPQRidot(eQ);
435 double rdotk = in.vPQRidot(eR);
436 double Ap = -0.25 * (pk*pdotk + qk*qdotk + rk*rdotk);
437 double Bp = 0.25 * (pk*qdotk - qk*pdotk);
438 double Cp = 0.25 * (pdotk*rk - pk*rdotk);
439 double Dp = 0.25 * (qk*rdotk - qdotk*rk);
440 double C2p = sin(rhok) / omegak;
441 double C3p = 2.0 * (1.0 - cos(rhok)) / (omegak*omegak);
442 double H = C1 + C4 * Ap;
443 double G = -C2p*rk - C3p*rdotk + C4*Bp;
444 double J = C2p*qk + C3p*qdotk - C4*Cp;
445 double K = C2p*pk + C3p*pdotk - C4*Dp;
446
447 cout << "q: " << q << endl;
448
449 // Warning! In the paper of Barker et al. the quaternion components are not
450 // ordered the same way as in JSBSim (see equations (2) and (3) of ref. [7]
451 // as well as the comment just below equation (3))
452 cout << "FORTRAN: " << H << " , " << K << " , " << J << " , " << -G << endl;*/
453 }
454 break; // The quaternion q is not normal so the normalization needs to be done.
455 case eNone: // do nothing, freeze rotational rate
456 break;
457 default:
458 break;
459 }
460
461 Integrand.Normalize();
462 }
463
464 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
465
UpdateLocationMatrices(void)466 void FGPropagate::UpdateLocationMatrices(void)
467 {
468 Tl2ec = VState.vLocation.GetTl2ec(); // local to ECEF transform
469 Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
470 Ti2l = Tec2l * Ti2ec; // ECI to local frame transform
471 Tl2i = Ti2l.Transposed(); // local to ECI transform
472 }
473
474 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
475
UpdateBodyMatrices(void)476 void FGPropagate::UpdateBodyMatrices(void)
477 {
478 Ti2b = VState.qAttitudeECI.GetT(); // ECI to body frame transform
479 Tb2i = Ti2b.Transposed(); // body to ECI frame transform
480 Tl2b = Ti2b * Tl2i; // local to body frame transform
481 Tb2l = Tl2b.Transposed(); // body to local frame transform
482 Tec2b = Ti2b * Tec2i; // ECEF to body frame transform
483 Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
484
485 Qec2b = Tec2b.GetQuaternion();
486 }
487
488 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
489
SetInertialOrientation(const FGQuaternion & Qi)490 void FGPropagate::SetInertialOrientation(const FGQuaternion& Qi)
491 {
492 VState.qAttitudeECI = Qi;
493 VState.qAttitudeECI.Normalize();
494 UpdateBodyMatrices();
495 VState.qAttitudeLocal = Tl2b.GetQuaternion();
496 CalculateQuatdot();
497 }
498
499 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
500
SetInertialVelocity(const FGColumnVector3 & Vi)501 void FGPropagate::SetInertialVelocity(const FGColumnVector3& Vi) {
502 VState.vInertialVelocity = Vi;
503 CalculateUVW();
504 vVel = Tb2l * VState.vUVW;
505 }
506
507 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
508
SetInertialRates(const FGColumnVector3 & vRates)509 void FGPropagate::SetInertialRates(const FGColumnVector3& vRates) {
510 VState.vPQRi = Ti2b * vRates;
511 VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
512 CalculateQuatdot();
513 }
514
515 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
516
RecomputeLocalTerrainVelocity()517 void FGPropagate::RecomputeLocalTerrainVelocity()
518 {
519 FGLocation contact;
520 FGColumnVector3 normal;
521 VState.vLocation.GetContactPoint(contact, normal, LocalTerrainVelocity,
522 LocalTerrainAngularVelocity);
523 }
524
525 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
526
SetTerrainElevation(double terrainElev)527 void FGPropagate::SetTerrainElevation(double terrainElev)
528 {
529 double radius = terrainElev + VState.vLocation.GetSeaLevelRadius();
530 FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(radius);
531 }
532
533 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
534
GetLocalTerrainRadius(void) const535 double FGPropagate::GetLocalTerrainRadius(void) const
536 {
537 return VState.vLocation.GetTerrainRadius();
538 }
539
540 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
541
GetDistanceAGL(void) const542 double FGPropagate::GetDistanceAGL(void) const
543 {
544 return VState.vLocation.GetAltitudeAGL();
545 }
546
547 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
548
GetDistanceAGLKm(void) const549 double FGPropagate::GetDistanceAGLKm(void) const
550 {
551 return VState.vLocation.GetAltitudeAGL()*0.0003048;
552 }
553
554 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
555
SetDistanceAGL(double tt)556 void FGPropagate::SetDistanceAGL(double tt)
557 {
558 VState.vLocation.SetAltitudeAGL(tt);
559 UpdateVehicleState();
560 }
561
562 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
563
SetDistanceAGLKm(double tt)564 void FGPropagate::SetDistanceAGLKm(double tt)
565 {
566 VState.vLocation.SetAltitudeAGL(tt*3280.8399);
567 UpdateVehicleState();
568 }
569
570 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
571
SetVState(const VehicleState & vstate)572 void FGPropagate::SetVState(const VehicleState& vstate)
573 {
574 //ToDo: Shouldn't all of these be set from the vstate vector passed in?
575 VState.vLocation = vstate.vLocation;
576 UpdateLocationMatrices();
577 SetInertialOrientation(vstate.qAttitudeECI);
578 RecomputeLocalTerrainVelocity();
579 VState.vUVW = vstate.vUVW;
580 vVel = Tb2l * VState.vUVW;
581 VState.vPQR = vstate.vPQR;
582 VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet;
583 VState.vInertialPosition = vstate.vInertialPosition;
584 CalculateQuatdot();
585 }
586
587 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
588
UpdateVehicleState(void)589 void FGPropagate::UpdateVehicleState(void)
590 {
591 RecomputeLocalTerrainVelocity();
592 VState.vInertialPosition = Tec2i * VState.vLocation;
593 UpdateLocationMatrices();
594 UpdateBodyMatrices();
595 vVel = Tb2l * VState.vUVW;
596 VState.qAttitudeLocal = Tl2b.GetQuaternion();
597 }
598
599 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
600
SetLocation(const FGLocation & l)601 void FGPropagate::SetLocation(const FGLocation& l)
602 {
603 VState.vLocation = l;
604 UpdateVehicleState();
605 }
606
607 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
608
GetEulerDeg(void) const609 FGColumnVector3 FGPropagate::GetEulerDeg(void) const
610 {
611 return VState.qAttitudeLocal.GetEuler() * radtodeg;
612 }
613
614 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
615
DumpState(void)616 void FGPropagate::DumpState(void)
617 {
618 cout << endl;
619 cout << fgblue
620 << "------------------------------------------------------------------" << reset << endl;
621 cout << highint
622 << "State Report at sim time: " << FDMExec->GetSimTime() << " seconds" << reset << endl;
623 cout << " " << underon
624 << "Position" << underoff << endl;
625 cout << " ECI: " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl;
626 cout << " ECEF: " << VState.vLocation << " (x,y,z, in ft)" << endl;
627 cout << " Local: " << VState.vLocation.GetGeodLatitudeDeg()
628 << ", " << VState.vLocation.GetLongitudeDeg()
629 << ", " << GetAltitudeASL() << " (geodetic lat, lon, alt ASL in deg and ft)" << endl;
630
631 cout << endl << " " << underon
632 << "Orientation" << underoff << endl;
633 cout << " ECI: " << VState.qAttitudeECI.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
634 cout << " Local: " << VState.qAttitudeLocal.GetEulerDeg().Dump(", ") << " (phi, theta, psi in deg)" << endl;
635
636 cout << endl << " " << underon
637 << "Velocity" << underoff << endl;
638 cout << " ECI: " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl;
639 cout << " ECEF: " << (Tb2ec * VState.vUVW).Dump(", ") << " (x,y,z in ft/s)" << endl;
640 cout << " Local: " << GetVel() << " (n,e,d in ft/sec)" << endl;
641 cout << " Body: " << GetUVW() << " (u,v,w in ft/sec)" << endl;
642
643 cout << endl << " " << underon
644 << "Body Rates (relative to given frame, expressed in body frame)" << underoff << endl;
645 cout << " ECI: " << (VState.vPQRi*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
646 cout << " ECEF: " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl;
647 }
648
649 //******************************************************************************
650
WriteStateFile(int num)651 void FGPropagate::WriteStateFile(int num)
652 {
653 sg_ofstream outfile;
654
655 if (num == 0) return;
656
657 SGPath path = FDMExec->GetFullAircraftPath();
658
659 if (path.isNull()) path = SGPath("initfile.");
660 else path.append("initfile.");
661
662 // Append sim time to the filename since there may be more than one created during a simulation run
663 path.concat(to_string((double)FDMExec->GetSimTime())+".xml");
664
665 switch(num) {
666 case 1:
667 outfile.open(path);
668 if (outfile.is_open()) {
669 outfile << "<?xml version=\"1.0\"?>" << endl;
670 outfile << "<initialize name=\"reset00\">" << endl;
671 outfile << " <ubody unit=\"FT/SEC\"> " << VState.vUVW(eU) << " </ubody> " << endl;
672 outfile << " <vbody unit=\"FT/SEC\"> " << VState.vUVW(eV) << " </vbody> " << endl;
673 outfile << " <wbody unit=\"FT/SEC\"> " << VState.vUVW(eW) << " </wbody> " << endl;
674 outfile << " <phi unit=\"DEG\"> " << VState.qAttitudeLocal.GetEuler(ePhi)*radtodeg << " </phi>" << endl;
675 outfile << " <theta unit=\"DEG\"> " << VState.qAttitudeLocal.GetEuler(eTht)*radtodeg << " </theta>" << endl;
676 outfile << " <psi unit=\"DEG\"> " << VState.qAttitudeLocal.GetEuler(ePsi)*radtodeg << " </psi>" << endl;
677 outfile << " <longitude unit=\"DEG\"> " << VState.vLocation.GetLongitudeDeg() << " </longitude>" << endl;
678 outfile << " <latitude unit=\"DEG\"> " << VState.vLocation.GetLatitudeDeg() << " </latitude>" << endl;
679 outfile << " <altitude unit=\"FT\"> " << GetDistanceAGL() << " </altitude>" << endl;
680 outfile << "</initialize>" << endl;
681 outfile.close();
682 } else {
683 cerr << "Could not open and/or write the state to the initial conditions file: "
684 << path << endl;
685 }
686 break;
687 case 2:
688 outfile.open(path);
689 if (outfile.is_open()) {
690 outfile << "<?xml version=\"1.0\"?>" << endl;
691 outfile << "<initialize name=\"IC File\" version=\"2.0\">" << endl;
692 outfile << "" << endl;
693 outfile << " <position frame=\"ECEF\">" << endl;
694 outfile << " <latitude unit=\"DEG\" type=\"geodetic\"> " << VState.vLocation.GetGeodLatitudeDeg() << " </latitude>" << endl;
695 outfile << " <longitude unit=\"DEG\"> " << VState.vLocation.GetLongitudeDeg() << " </longitude>" << endl;
696 outfile << " <altitudeMSL unit=\"FT\"> " << GetAltitudeASL() << " </altitudeMSL>" << endl;
697 outfile << " </position>" << endl;
698 outfile << "" << endl;
699 outfile << " <orientation unit=\"DEG\" frame=\"LOCAL\">" << endl;
700 outfile << " <yaw> " << VState.qAttitudeLocal.GetEulerDeg(eYaw) << " </yaw>" << endl;
701 outfile << " <pitch> " << VState.qAttitudeLocal.GetEulerDeg(ePitch) << " </pitch>" << endl;
702 outfile << " <roll> " << VState.qAttitudeLocal.GetEulerDeg(eRoll) << " </roll>" << endl;
703 outfile << " </orientation>" << endl;
704 outfile << "" << endl;
705 outfile << " <velocity unit=\"FT/SEC\" frame=\"LOCAL\">" << endl;
706 outfile << " <x> " << GetVel(eNorth) << " </x>" << endl;
707 outfile << " <y> " << GetVel(eEast) << " </y>" << endl;
708 outfile << " <z> " << GetVel(eDown) << " </z>" << endl;
709 outfile << " </velocity>" << endl;
710 outfile << "" << endl;
711 outfile << " <attitude_rate unit=\"DEG/SEC\" frame=\"BODY\">" << endl;
712 outfile << " <roll> " << (VState.vPQR*radtodeg)(eRoll) << " </roll>" << endl;
713 outfile << " <pitch> " << (VState.vPQR*radtodeg)(ePitch) << " </pitch>" << endl;
714 outfile << " <yaw> " << (VState.vPQR*radtodeg)(eYaw) << " </yaw>" << endl;
715 outfile << " </attitude_rate>" << endl;
716 outfile << "" << endl;
717 outfile << "</initialize>" << endl;
718 outfile.close();
719 } else {
720 cerr << "Could not open and/or write the state to the initial conditions file: "
721 << path << endl;
722 }
723 break;
724 default:
725 cerr << "When writing a state file, the supplied value must be 1 or 2 for the version number of the resulting IC file" << endl;
726 }
727 }
728
729 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
730
bind(void)731 void FGPropagate::bind(void)
732 {
733 typedef double (FGPropagate::*PMF)(int) const;
734 typedef int (FGPropagate::*iPMF)(void) const;
735
736 PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
737
738 PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
739 PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
740 PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
741
742 PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
743 PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
744 PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
745
746 PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
747 PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
748 PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
749
750 PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
751 PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
752 PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
753
754 PropertyManager->Tie("velocities/eci-x-fps", this, eX, (PMF)&FGPropagate::GetInertialVelocity);
755 PropertyManager->Tie("velocities/eci-y-fps", this, eY, (PMF)&FGPropagate::GetInertialVelocity);
756 PropertyManager->Tie("velocities/eci-z-fps", this, eZ, (PMF)&FGPropagate::GetInertialVelocity);
757
758 PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
759 PropertyManager->Tie("velocities/ned-velocity-mag-fps", this, &FGPropagate::GetNEDVelocityMagnitude);
760
761 PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
762 PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
763 PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude, false);
764 PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude, false);
765 PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg, false);
766 PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg, false);
767 PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
768 PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
769 PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
770 PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
771 PropertyManager->Tie("position/geod-alt-km", this, &FGPropagate::GetGeodeticAltitudeKm);
772 PropertyManager->Tie("position/h-agl-km", this, &FGPropagate::GetDistanceAGLKm, &FGPropagate::SetDistanceAGLKm);
773 PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
774 PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
775 &FGPropagate::GetTerrainElevation,
776 &FGPropagate::SetTerrainElevation, false);
777
778 PropertyManager->Tie("position/eci-x-ft", this, eX, (PMF)&FGPropagate::GetInertialPosition);
779 PropertyManager->Tie("position/eci-y-ft", this, eY, (PMF)&FGPropagate::GetInertialPosition);
780 PropertyManager->Tie("position/eci-z-ft", this, eZ, (PMF)&FGPropagate::GetInertialPosition);
781
782 PropertyManager->Tie("position/ecef-x-ft", this, eX, (PMF)&FGPropagate::GetLocation);
783 PropertyManager->Tie("position/ecef-y-ft", this, eY, (PMF)&FGPropagate::GetLocation);
784 PropertyManager->Tie("position/ecef-z-ft", this, eZ, (PMF)&FGPropagate::GetLocation);
785
786 PropertyManager->Tie("position/epa-rad", this, &FGPropagate::GetEarthPositionAngle);
787 PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
788
789 PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
790 PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
791 PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
792
793 PropertyManager->Tie("attitude/phi-deg", this, (int)ePhi, (PMF)&FGPropagate::GetEulerDeg);
794 PropertyManager->Tie("attitude/theta-deg", this, (int)eTht, (PMF)&FGPropagate::GetEulerDeg);
795 PropertyManager->Tie("attitude/psi-deg", this, (int)ePsi, (PMF)&FGPropagate::GetEulerDeg);
796
797 PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
798 PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
799 PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
800
801 PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
802 PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
803 PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
804 PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
805
806 PropertyManager->Tie("simulation/write-state-file", this, (iPMF)0, &FGPropagate::WriteStateFile);
807 }
808
809 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
810 // The bitmasked value choices are as follows:
811 // unset: In this case (the default) JSBSim would only print
812 // out the normally expected messages, essentially echoing
813 // the config files as they are read. If the environment
814 // variable is not set, debug_lvl is set to 1 internally
815 // 0: This requests JSBSim not to output any messages
816 // whatsoever.
817 // 1: This value explicity requests the normal JSBSim
818 // startup messages
819 // 2: This value asks for a message to be printed out when
820 // a class is instantiated
821 // 4: When this value is set, a message is displayed when a
822 // FGModel object executes its Run() method
823 // 8: When this value is set, various runtime state variables
824 // are printed out periodically
825 // 16: When set various parameters are sanity checked and
826 // a message is printed out when they go out of bounds
827
Debug(int from)828 void FGPropagate::Debug(int from)
829 {
830 if (debug_lvl <= 0) return;
831
832 if (debug_lvl & 1) { // Standard console startup message output
833 if (from == 0) { // Constructor
834
835 }
836 }
837 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
838 if (from == 0) cout << "Instantiated: FGPropagate" << endl;
839 if (from == 1) cout << "Destroyed: FGPropagate" << endl;
840 }
841 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
842 }
843 if (debug_lvl & 8 && from == 2) { // Runtime state variables
844 cout << endl << fgblue << highint << left
845 << " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
846 << reset << endl;
847 cout << endl;
848 cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
849 << GetEarthPositionAngleDeg() << endl;
850 cout << endl;
851 cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
852 cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
853 cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
854 cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
855 cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
856 cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
857 cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
858 // cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
859 cout << endl;
860 cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
861 << reset << endl << Tec2b.Dump("\t", " ") << endl;
862 cout << highint << " Associated Euler angles (deg): " << setw(8)
863 << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
864 << endl << endl;
865
866 cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
867 << reset << endl << Tb2ec.Dump("\t", " ") << endl;
868 cout << highint << " Associated Euler angles (deg): " << setw(8)
869 << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
870 << endl << endl;
871
872 cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
873 << reset << endl << Tl2b.Dump("\t", " ") << endl;
874 cout << highint << " Associated Euler angles (deg): " << setw(8)
875 << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
876 << endl << endl;
877
878 cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
879 << reset << endl << Tb2l.Dump("\t", " ") << endl;
880 cout << highint << " Associated Euler angles (deg): " << setw(8)
881 << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
882 << endl << endl;
883
884 cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
885 << reset << endl << Tl2ec.Dump("\t", " ") << endl;
886 cout << highint << " Associated Euler angles (deg): " << setw(8)
887 << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
888 << endl << endl;
889
890 cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
891 << reset << endl << Tec2l.Dump("\t", " ") << endl;
892 cout << highint << " Associated Euler angles (deg): " << setw(8)
893 << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
894 << endl << endl;
895
896 cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
897 << reset << endl << Tec2i.Dump("\t", " ") << endl;
898 cout << highint << " Associated Euler angles (deg): " << setw(8)
899 << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
900 << endl << endl;
901
902 cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
903 << reset << endl << Ti2ec.Dump("\t", " ") << endl;
904 cout << highint << " Associated Euler angles (deg): " << setw(8)
905 << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
906 << endl << endl;
907
908 cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
909 << reset << endl << Ti2b.Dump("\t", " ") << endl;
910 cout << highint << " Associated Euler angles (deg): " << setw(8)
911 << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
912 << endl << endl;
913
914 cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
915 << reset << endl << Tb2i.Dump("\t", " ") << endl;
916 cout << highint << " Associated Euler angles (deg): " << setw(8)
917 << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
918 << endl << endl;
919
920 cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
921 << reset << endl << Ti2l.Dump("\t", " ") << endl;
922 cout << highint << " Associated Euler angles (deg): " << setw(8)
923 << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
924 << endl << endl;
925
926 cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
927 << reset << endl << Tl2i.Dump("\t", " ") << endl;
928 cout << highint << " Associated Euler angles (deg): " << setw(8)
929 << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
930 << endl << endl;
931
932 cout << setprecision(6); // reset the output stream
933 }
934 if (debug_lvl & 16) { // Sanity checking
935 if (from == 2) { // State sanity checking
936 if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
937 cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
938 exit(-1);
939 }
940 if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
941 cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
942 exit(-1);
943 }
944 if (fabs(GetDistanceAGL()) > 1e10) {
945 cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
946 exit(-1);
947 }
948 }
949 }
950 if (debug_lvl & 64) {
951 if (from == 0) { // Constructor
952 }
953 }
954 }
955 }
956