Branch data Line data Source code
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 Software
13 : : Foundation; either version 2 of the License, or (at your option) any later
14 : : 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 with
22 : : this program; if not, write to the Free Software Foundation, Inc., 59 Temple
23 : : Place - Suite 330, Boston, MA 02111-1307, USA.
24 : :
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.
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] Erin Catto, "Iterative Dynamics with Temporal Coherence", February 22, 2005
52 : :
53 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
54 : : INCLUDES
55 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
56 : :
57 : : #include <cmath>
58 : : #include <cstdlib>
59 : : #include <iostream>
60 : : #include <iomanip>
61 : :
62 : : #include "FGPropagate.h"
63 : : #include "FGGroundReactions.h"
64 : : #include "FGFDMExec.h"
65 : : #include "FGAircraft.h"
66 : : #include "FGMassBalance.h"
67 : : #include "FGInertial.h"
68 : : #include "input_output/FGPropertyManager.h"
69 : :
70 : : using namespace std;
71 : :
72 : : namespace JSBSim {
73 : :
74 : : static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.60 2010/08/12 19:11:22 andgi Exp $";
75 : : static const char *IdHdr = ID_PROPAGATE;
76 : :
77 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
78 : : CLASS IMPLEMENTATION
79 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
80 : :
81 : 1 : FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
82 : : {
83 : 1 : Debug(0);
84 : 1 : Name = "FGPropagate";
85 : 1 : gravType = gtWGS84;
86 : :
87 : 1 : vPQRdot.InitMatrix();
88 : 1 : vQtrndot = FGQuaternion(0,0,0);
89 : 1 : vUVWdot.InitMatrix();
90 : 1 : vInertialVelocity.InitMatrix();
91 : :
92 : 1 : integrator_rotational_rate = eAdamsBashforth2;
93 : 1 : integrator_translational_rate = eTrapezoidal;
94 : 1 : integrator_rotational_position = eAdamsBashforth2;
95 : 1 : integrator_translational_position = eTrapezoidal;
96 : :
97 : 1 : VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
98 : 1 : VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
99 : 1 : VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
100 : 1 : VState.dqQtrndot.resize(4, FGQuaternion(0.0,0.0,0.0));
101 : :
102 : 1 : bind();
103 : 1 : Debug(0);
104 : 1 : }
105 : :
106 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
107 : :
108 : 1 : FGPropagate::~FGPropagate(void)
109 : : {
110 : 1 : Debug(1);
111 : 1 : }
112 : :
113 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
114 : :
115 : 1 : bool FGPropagate::InitModel(void)
116 : : {
117 [ - + ]: 1 : if (!FGModel::InitModel()) return false;
118 : :
119 : : // For initialization ONLY:
120 : 2 : SeaLevelRadius = LocalTerrainRadius = Inertial->GetRefRadius();
121 : :
122 : 1 : VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
123 : 1 : VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
124 : 2 : vOmegaEarth = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
125 : :
126 : 1 : vPQRdot.InitMatrix();
127 : 1 : vQtrndot = FGQuaternion(0,0,0);
128 : 1 : vUVWdot.InitMatrix();
129 : 1 : vInertialVelocity.InitMatrix();
130 : :
131 : 1 : VState.dqPQRdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
132 : 1 : VState.dqUVWdot.resize(4, FGColumnVector3(0.0,0.0,0.0));
133 : 1 : VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0));
134 : 1 : VState.dqQtrndot.resize(4, FGColumnVector3(0.0,0.0,0.0));
135 : :
136 : 1 : integrator_rotational_rate = eAdamsBashforth2;
137 : 1 : integrator_translational_rate = eTrapezoidal;
138 : 1 : integrator_rotational_position = eAdamsBashforth2;
139 : 1 : integrator_translational_position = eTrapezoidal;
140 : :
141 : 1 : return true;
142 : : }
143 : :
144 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
145 : :
146 : 1 : void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
147 : : {
148 : : SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
149 : 1 : SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
150 : :
151 : 1 : VehicleRadius = GetRadius();
152 : 1 : radInv = 1.0/VehicleRadius;
153 : :
154 : : // Initialize the State Vector elements and the transformation matrices
155 : :
156 : : // Set the position lat/lon/radius
157 : : VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
158 : : FGIC->GetLatitudeRadIC(),
159 : 1 : FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
160 : :
161 : 2 : VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle());
162 : :
163 : 1 : Ti2ec = GetTi2ec(); // ECI to ECEF transform
164 : 1 : Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
165 : :
166 : 1 : Tl2ec = GetTl2ec(); // local to ECEF transform
167 : 1 : Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
168 : :
169 : 1 : Ti2l = GetTi2l();
170 : 1 : Tl2i = Ti2l.Transposed();
171 : :
172 : : // Set the orientation from the euler angles (is normalized within the
173 : : // constructor). The Euler angles represent the orientation of the body
174 : : // frame relative to the local frame.
175 : : VState.qAttitudeLocal = FGQuaternion( FGIC->GetPhiRadIC(),
176 : : FGIC->GetThetaRadIC(),
177 : 1 : FGIC->GetPsiRadIC() );
178 : :
179 : 2 : VState.qAttitudeECI = Ti2l.GetQuaternion()*VState.qAttitudeLocal;
180 : :
181 : 1 : Ti2b = GetTi2b(); // ECI to body frame transform
182 : 1 : Tb2i = Ti2b.Transposed(); // body to ECI frame transform
183 : :
184 : 1 : Tl2b = VState.qAttitudeLocal; // local to body frame transform
185 : 1 : Tb2l = Tl2b.Transposed(); // body to local frame transform
186 : :
187 : 1 : Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
188 : 1 : Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
189 : :
190 : : // Set the velocities in the instantaneus body frame
191 : : VState.vUVW = FGColumnVector3( FGIC->GetUBodyFpsIC(),
192 : : FGIC->GetVBodyFpsIC(),
193 : 1 : FGIC->GetWBodyFpsIC() );
194 : :
195 : 1 : VState.vInertialPosition = Tec2i * VState.vLocation;
196 : :
197 : : // Compute the local frame ECEF velocity
198 : 1 : vVel = Tb2l * VState.vUVW;
199 : :
200 : : // Refer to Stevens and Lewis, 1.5-14a, pg. 49.
201 : : // This is the rotation rate of the "Local" frame, expressed in the local frame.
202 : :
203 : : FGColumnVector3 vOmegaLocal = FGColumnVector3(
204 : : radInv*vVel(eEast),
205 : : -radInv*vVel(eNorth),
206 : 5 : -radInv*vVel(eEast)*VState.vLocation.GetTanLatitude() );
207 : :
208 : : // Set the angular velocities of the body frame relative to the ECEF frame,
209 : : // expressed in the body frame. Effectively, this is:
210 : : // w_b/e = w_b/l + w_l/e
211 : : VState.vPQR = FGColumnVector3( FGIC->GetPRadpsIC(),
212 : : FGIC->GetQRadpsIC(),
213 : 1 : FGIC->GetRRadpsIC() ) + Tl2b*vOmegaLocal;
214 : :
215 : 1 : VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
216 : :
217 : : // Make an initial run and set past values
218 : 1 : CalculatePQRdot(); // Angular rate derivative
219 : 1 : CalculateUVWdot(); // Translational rate derivative
220 : 1 : ResolveFrictionForces(0.); // Update rate derivatives with friction forces
221 : 1 : CalculateQuatdot(); // Angular orientation derivative
222 : 1 : CalculateInertialVelocity(); // Translational position derivative
223 : :
224 : : // Initialize past values deques
225 : 1 : VState.dqPQRdot.clear();
226 : 1 : VState.dqUVWdot.clear();
227 : 1 : VState.dqInertialVelocity.clear();
228 : 1 : VState.dqQtrndot.clear();
229 [ + + ]: 5 : for (int i=0; i<4; i++) {
230 : 4 : VState.dqPQRdot.push_front(vPQRdot);
231 : 4 : VState.dqUVWdot.push_front(vUVWdot);
232 : 4 : VState.dqInertialVelocity.push_front(VState.vInertialVelocity);
233 : 4 : VState.dqQtrndot.push_front(vQtrndot);
234 : : }
235 : :
236 : : // Recompute the LocalTerrainRadius.
237 : 1 : RecomputeLocalTerrainRadius();
238 : 1 : }
239 : :
240 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
241 : : /*
242 : : Purpose: Called on a schedule to perform EOM integration
243 : : Notes: [JB] Run in standalone mode, SeaLevelRadius will be reference radius.
244 : : In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
245 : :
246 : : At the top of this Run() function, see several "shortcuts" (or, aliases) being
247 : : set up for use later, rather than using the longer class->function() notation.
248 : :
249 : : This propagation is done using the current state values
250 : : and current derivatives. Based on these values we compute an approximation to the
251 : : state values for (now + dt).
252 : :
253 : : In the code below, variables named beginning with a small "v" refer to a
254 : : a column vector, variables named beginning with a "T" refer to a transformation
255 : : matrix. ECEF refers to Earth Centered Earth Fixed. ECI refers to Earth Centered
256 : : Inertial.
257 : :
258 : : */
259 : :
260 : 54005 : bool FGPropagate::Run(void)
261 : : {
262 [ - + ]: 54005 : if (FGModel::Run()) return true; // Fast return if we have nothing to do ...
263 [ - + ]: 54005 : if (FDMExec->Holding()) return false;
264 : :
265 : 108010 : double dt = FDMExec->GetDeltaT()*rate; // The 'stepsize'
266 : :
267 : 54005 : RunPreFunctions();
268 : :
269 : : // Calculate state derivatives
270 : 54005 : CalculatePQRdot(); // Angular rate derivative
271 : 54005 : CalculateUVWdot(); // Translational rate derivative
272 : 54005 : ResolveFrictionForces(dt); // Update rate derivatives with friction forces
273 : 54005 : CalculateQuatdot(); // Angular orientation derivative
274 : 54005 : CalculateInertialVelocity(); // Translational position derivative
275 : :
276 : : // Propagate rotational / translational velocity, angular /translational position, respectively.
277 : 54005 : Integrate(VState.vPQRi, vPQRdot, VState.dqPQRdot, dt, integrator_rotational_rate);
278 : 54005 : Integrate(VState.vUVW, vUVWdot, VState.dqUVWdot, dt, integrator_translational_rate);
279 : 54005 : Integrate(VState.qAttitudeECI, vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
280 : 54005 : Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
281 : :
282 : 54005 : VState.qAttitudeECI.Normalize(); // Normalize the ECI Attitude quaternion
283 : :
284 : 108010 : VState.vLocation.SetEarthPositionAngle(Inertial->GetEarthPositionAngle()); // Update the Earth position angle (EPA)
285 : :
286 : : // Update the "Location-based" transformation matrices from the vLocation vector.
287 : :
288 : 54005 : Ti2ec = GetTi2ec(); // ECI to ECEF transform
289 : 54005 : Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
290 : 54005 : Tl2ec = GetTl2ec(); // local to ECEF transform
291 : 54005 : Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
292 : 54005 : Ti2l = GetTi2l();
293 : 54005 : Tl2i = Ti2l.Transposed();
294 : :
295 : : // Update the "Orientation-based" transformation matrices from the orientation quaternion
296 : :
297 : 54005 : Ti2b = GetTi2b(); // ECI to body frame transform
298 : 54005 : Tb2i = Ti2b.Transposed(); // body to ECI frame transform
299 : 54005 : Tl2b = Ti2b*Tl2i; // local to body frame transform
300 : 54005 : Tb2l = Tl2b.Transposed(); // body to local frame transform
301 : 54005 : Tec2b = Tl2b * Tec2l; // ECEF to body frame transform
302 : 54005 : Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
303 : :
304 : : // Set auxililary state variables
305 : 54005 : VState.vLocation = Ti2ec*VState.vInertialPosition;
306 : 54005 : RecomputeLocalTerrainRadius();
307 : :
308 : 54005 : VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
309 : 54005 : radInv = 1.0/VehicleRadius;
310 : :
311 : 54005 : VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
312 : :
313 : 54005 : VState.qAttitudeLocal = Tl2b.GetQuaternion();
314 : :
315 : : // Compute vehicle velocity wrt ECEF frame, expressed in Local horizontal frame.
316 : 54005 : vVel = Tb2l * VState.vUVW;
317 : :
318 : 54005 : RunPostFunctions();
319 : :
320 : 54005 : Debug(2);
321 : 54005 : return false;
322 : : }
323 : :
324 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
325 : : // Compute body frame rotational accelerations based on the current body moments
326 : : //
327 : : // vPQRdot is the derivative of the absolute angular velocity of the vehicle
328 : : // (body rate with respect to the inertial frame), expressed in the body frame,
329 : : // where the derivative is taken in the body frame.
330 : : // J is the inertia matrix
331 : : // Jinv is the inverse inertia matrix
332 : : // vMoments is the moment vector in the body frame
333 : : // VState.vPQRi is the total inertial angular velocity of the vehicle
334 : : // expressed in the body frame.
335 : : // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
336 : : // Second edition (2004), eqn 1.5-16e (page 50)
337 : :
338 : 54006 : void FGPropagate::CalculatePQRdot(void)
339 : : {
340 : 108012 : const FGColumnVector3& vMoments = Aircraft->GetMoments(); // current moments
341 : 108012 : const FGMatrix33& J = MassBalance->GetJ(); // inertia matrix
342 : 108012 : const FGMatrix33& Jinv = MassBalance->GetJinv(); // inertia matrix inverse
343 : :
344 : : // Compute body frame rotational accelerations based on the current body
345 : : // moments and the total inertial angular velocity expressed in the body
346 : : // frame.
347 : :
348 : 54006 : vPQRdot = Jinv*(vMoments - VState.vPQRi*(J*VState.vPQRi));
349 : 54006 : }
350 : :
351 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
352 : : // Compute the quaternion orientation derivative
353 : : //
354 : : // vQtrndot is the quaternion derivative.
355 : : // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
356 : : // Second edition (2004), eqn 1.5-16b (page 50)
357 : :
358 : 54006 : void FGPropagate::CalculateQuatdot(void)
359 : : {
360 : : // Compute quaternion orientation derivative on current body rates
361 : 54006 : vQtrndot = VState.qAttitudeECI.GetQDot( VState.vPQRi);
362 : 54006 : }
363 : :
364 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
365 : : // This set of calculations results in the body frame accelerations being
366 : : // computed.
367 : : // Compute body frame accelerations based on the current body forces.
368 : : // Include centripetal and coriolis accelerations.
369 : : // vOmegaEarth is the Earth angular rate - expressed in the inertial frame -
370 : : // so it has to be transformed to the body frame. More completely,
371 : : // vOmegaEarth is the rate of the ECEF frame relative to the Inertial
372 : : // frame (ECI), expressed in the Inertial frame.
373 : : // vForces is the total force on the vehicle in the body frame.
374 : : // VState.vPQR is the vehicle body rate relative to the ECEF frame, expressed
375 : : // in the body frame.
376 : : // VState.vUVW is the vehicle velocity relative to the ECEF frame, expressed
377 : : // in the body frame.
378 : : // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
379 : : // Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
380 : :
381 : 54006 : void FGPropagate::CalculateUVWdot(void)
382 : : {
383 : 108012 : double mass = MassBalance->GetMass(); // mass
384 : 108012 : const FGColumnVector3& vForces = Aircraft->GetForces(); // current forces
385 : :
386 : 54006 : vUVWdot = vForces/mass - (VState.vPQR + 2.0*(Ti2b *vOmegaEarth)) * VState.vUVW;
387 : :
388 : : // Include Centripetal acceleration.
389 : 54006 : vUVWdot -= Ti2b * (vOmegaEarth*(vOmegaEarth*VState.vInertialPosition));
390 : :
391 : : // Include Gravitation accel
392 [ - + - ]: 54006 : switch (gravType) {
393 : : case gtStandard:
394 : 0 : vGravAccel = Tl2b * FGColumnVector3( 0.0, 0.0, Inertial->GetGAccel(VehicleRadius) );
395 : : break;
396 : : case gtWGS84:
397 : 54006 : vGravAccel = Tec2b * Inertial->GetGravityJ2(VState.vLocation);
398 : : break;
399 : : }
400 : :
401 : 54006 : vUVWdot += vGravAccel;
402 : 54006 : }
403 : :
404 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
405 : : // Transform the velocity vector of the body relative to the origin (Earth
406 : : // center) to be expressed in the inertial frame, and add the vehicle velocity
407 : : // contribution due to the rotation of the planet.
408 : : // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
409 : : // Second edition (2004), eqn 1.5-16c (page 50)
410 : :
411 : 54006 : void FGPropagate::CalculateInertialVelocity(void)
412 : : {
413 : 54006 : VState.vInertialVelocity = Tb2i * VState.vUVW + (vOmegaEarth * VState.vInertialPosition);
414 : 54006 : }
415 : :
416 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
417 : :
418 : : void FGPropagate::Integrate( FGColumnVector3& Integrand,
419 : : FGColumnVector3& Val,
420 : : deque <FGColumnVector3>& ValDot,
421 : : double dt,
422 : 162015 : eIntegrateType integration_type)
423 : : {
424 : 162015 : ValDot.push_front(Val);
425 : : ValDot.pop_back();
426 : :
427 [ - + + - : 162015 : switch(integration_type) {
- - ]
428 : 0 : case eRectEuler: Integrand += dt*ValDot[0];
429 : : break;
430 : 54005 : case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
431 : : break;
432 : 108010 : case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
433 : : break;
434 : 0 : case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
435 : : break;
436 : 0 : case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
437 : : break;
438 : : case eNone: // do nothing, freeze translational rate
439 : : break;
440 : : }
441 : 162015 : }
442 : :
443 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
444 : :
445 : : void FGPropagate::Integrate( FGQuaternion& Integrand,
446 : : FGQuaternion& Val,
447 : : deque <FGQuaternion>& ValDot,
448 : : double dt,
449 : 54005 : eIntegrateType integration_type)
450 : : {
451 : 54005 : ValDot.push_front(Val);
452 : : ValDot.pop_back();
453 : :
454 [ - - + - : 54005 : switch(integration_type) {
- - ]
455 : 0 : case eRectEuler: Integrand += dt*ValDot[0];
456 : : break;
457 : 0 : case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
458 : : break;
459 : 54005 : case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
460 : : break;
461 : 0 : case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
462 : : break;
463 : 0 : case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
464 : : break;
465 : : case eNone: // do nothing, freeze rotational rate
466 : : break;
467 : : }
468 : 54005 : }
469 : :
470 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
471 : : // Resolves the contact forces just before integrating the EOM.
472 : : // This routine is using Lagrange multipliers and the projected Gauss-Seidel
473 : : // (PGS) method.
474 : : // Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence",
475 : : // February 22, 2005
476 : : // In JSBSim there is only one rigid body (the aircraft) and there can be
477 : : // multiple points of contact between the aircraft and the ground. As a
478 : : // consequence our matrix J*M^-1*J^T is not sparse and the algorithm described
479 : : // in Catto's paper has been adapted accordingly.
480 : :
481 : 54006 : void FGPropagate::ResolveFrictionForces(double dt)
482 : : {
483 : 108012 : const double invMass = 1.0 / MassBalance->GetMass();
484 : 108012 : const FGMatrix33& Jinv = MassBalance->GetJinv();
485 : : vector <FGColumnVector3> JacF, JacM;
486 : 54006 : FGColumnVector3 vdot, wdot;
487 : 54006 : FGColumnVector3 Fc, Mc;
488 : 54006 : int n = 0, i;
489 : :
490 : : // Compiles data from the ground reactions to build up the jacobian matrix
491 [ - + ]: 54006 : for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, n++) {
492 : 0 : JacF.push_back((*it)->ForceJacobian);
493 : 0 : JacM.push_back((*it)->MomentJacobian);
494 : : }
495 : :
496 : : // If no gears are in contact with the ground then return
497 [ - + ]: 108012 : if (!n) return;
498 : :
499 : 0 : vector<double> a(n*n); // Will contain J*M^-1*J^T
500 : 0 : vector<double> eta(n);
501 : 0 : vector<double> lambda(n);
502 : 0 : vector<double> lambdaMin(n);
503 : 0 : vector<double> lambdaMax(n);
504 : :
505 : : // Initializes the Lagrange multipliers
506 : 0 : i = 0;
507 [ # # ]: 0 : for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it, i++) {
508 : 0 : lambda[i] = (*it)->value;
509 : 0 : lambdaMax[i] = (*it)->Max;
510 : 0 : lambdaMin[i] = (*it)->Min;
511 : : }
512 : :
513 : 0 : vdot = vUVWdot;
514 : 0 : wdot = vPQRdot;
515 : :
516 [ # # ]: 0 : if (dt > 0.) {
517 : : // First compute the ground velocity below the aircraft center of gravity
518 : 0 : FGLocation contact;
519 : 0 : FGColumnVector3 normal, cvel;
520 : :
521 : : // Instruct the algorithm to zero out the relative movement between the
522 : : // aircraft and the ground.
523 : 0 : vdot += (VState.vUVW - Tec2b * cvel) / dt;
524 : 0 : wdot += VState.vPQR / dt;
525 : : }
526 : :
527 : : // Assemble the linear system of equations
528 [ # # ]: 0 : for (i=0; i < n; i++) {
529 [ # # ]: 0 : for (int j=0; j < i; j++)
530 : 0 : a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of J^T*M^-1*J
531 [ # # ]: 0 : for (int j=i; j < n; j++)
532 : 0 : a[i*n+j] = DotProduct(JacF[i],invMass*JacF[j])+DotProduct(JacM[i],Jinv*JacM[j]);
533 : : }
534 : :
535 : : // Prepare the linear system for the Gauss-Seidel algorithm :
536 : : // divide every line of 'a' and eta by a[i,i]. This is in order to save
537 : : // a division computation at each iteration of Gauss-Seidel.
538 [ # # ]: 0 : for (i=0; i < n; i++) {
539 : 0 : double d = 1.0 / a[i*n+i];
540 : :
541 : 0 : eta[i] = -(DotProduct(JacF[i],vdot)+DotProduct(JacM[i],wdot))*d;
542 [ # # ]: 0 : for (int j=0; j < n; j++)
543 : 0 : a[i*n+j] *= d;
544 : : }
545 : :
546 : : // Resolve the Lagrange multipliers with the projected Gauss-Seidel method
547 [ # # ]: 0 : for (int iter=0; iter < 50; iter++) {
548 : 0 : double norm = 0.;
549 : :
550 [ # # ]: 0 : for (i=0; i < n; i++) {
551 : 0 : double lambda0 = lambda[i];
552 : 0 : double dlambda = eta[i];
553 : :
554 [ # # ]: 0 : for (int j=0; j < n; j++)
555 : 0 : dlambda -= a[i*n+j]*lambda[j];
556 : :
557 : 0 : lambda[i] = Constrain(lambdaMin[i], lambda0+dlambda, lambdaMax[i]);
558 : 0 : dlambda = lambda[i] - lambda0;
559 : :
560 : 0 : norm += fabs(dlambda);
561 : : }
562 : :
563 [ # # ]: 0 : if (norm < 1E-5) break;
564 : : }
565 : :
566 : : // Calculate the total friction forces and moments
567 : :
568 : : Fc.InitMatrix();
569 : : Mc.InitMatrix();
570 : :
571 [ # # ]: 0 : for (i=0; i< n; i++) {
572 : 0 : Fc += lambda[i]*JacF[i];
573 : 0 : Mc += lambda[i]*JacM[i];
574 : : }
575 : :
576 : 0 : vUVWdot += invMass * Fc;
577 : 0 : vPQRdot += Jinv * Mc;
578 : :
579 : : // Save the value of the Lagrange multipliers to accelerate the convergence
580 : : // of the Gauss-Seidel algorithm at next iteration.
581 : 0 : i = 0;
582 [ # # ]: 0 : for (MultiplierIterator it=MultiplierIterator(GroundReactions); *it; ++it)
583 : 0 : (*it)->value = lambda[i++];
584 : :
585 : 108012 : GroundReactions->UpdateForcesAndMoments();
586 : : }
587 : :
588 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
589 : :
590 : 0 : void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
591 : 0 : VState.qAttitudeECI = Qi;
592 : 0 : }
593 : :
594 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
595 : :
596 : 0 : void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
597 : 0 : VState.vInertialVelocity = Vi;
598 : 0 : }
599 : :
600 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
601 : :
602 : 54006 : void FGPropagate::RecomputeLocalTerrainRadius(void)
603 : : {
604 : 108012 : double t = FDMExec->GetSimTime();
605 : :
606 : : // Get the LocalTerrain radius.
607 : 54006 : FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
608 : 108012 : LocalTerrainRadius = contactloc.GetRadius();
609 : 54006 : }
610 : :
611 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
612 : :
613 : 1 : void FGPropagate::SetTerrainElevation(double terrainElev)
614 : : {
615 : 1 : LocalTerrainRadius = terrainElev + SeaLevelRadius;
616 : 1 : FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
617 : 1 : }
618 : :
619 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
620 : :
621 : 4910 : double FGPropagate::GetTerrainElevation(void) const
622 : : {
623 : 4910 : return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
624 : : }
625 : :
626 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
627 : : //Todo: when should this be called - when should the new EPA be used to
628 : : // calculate the transformation matrix, so that the matrix is not a step
629 : : // ahead of the sim and the associated calculations?
630 : 0 : const FGMatrix33& FGPropagate::GetTi2ec(void)
631 : : {
632 : 54006 : return VState.vLocation.GetTi2ec();
633 : : }
634 : :
635 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
636 : :
637 : 0 : const FGMatrix33& FGPropagate::GetTec2i(void)
638 : : {
639 : 0 : return VState.vLocation.GetTec2i();
640 : : }
641 : :
642 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
643 : :
644 : 2 : void FGPropagate::SetAltitudeASL(double altASL)
645 : : {
646 : 2 : VState.vLocation.SetRadius( altASL + SeaLevelRadius );
647 : 2 : }
648 : :
649 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
650 : :
651 : 1 : double FGPropagate::GetLocalTerrainRadius(void) const
652 : : {
653 : 1 : return LocalTerrainRadius;
654 : : }
655 : :
656 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
657 : :
658 : 117874 : double FGPropagate::GetDistanceAGL(void) const
659 : : {
660 : 117874 : return VState.vLocation.GetRadius() - LocalTerrainRadius;
661 : : }
662 : :
663 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
664 : :
665 : 0 : void FGPropagate::SetDistanceAGL(double tt)
666 : : {
667 : 0 : VState.vLocation.SetRadius( tt + LocalTerrainRadius );
668 : 0 : }
669 : :
670 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
671 : :
672 : 1 : void FGPropagate::bind(void)
673 : : {
674 : : typedef double (FGPropagate::*PMF)(int) const;
675 : :
676 : 1 : PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot);
677 : :
678 : 1 : PropertyManager->Tie("velocities/v-north-fps", this, eNorth, (PMF)&FGPropagate::GetVel);
679 : 1 : PropertyManager->Tie("velocities/v-east-fps", this, eEast, (PMF)&FGPropagate::GetVel);
680 : 1 : PropertyManager->Tie("velocities/v-down-fps", this, eDown, (PMF)&FGPropagate::GetVel);
681 : :
682 : 1 : PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);
683 : 1 : PropertyManager->Tie("velocities/v-fps", this, eV, (PMF)&FGPropagate::GetUVW);
684 : 1 : PropertyManager->Tie("velocities/w-fps", this, eW, (PMF)&FGPropagate::GetUVW);
685 : :
686 : 1 : PropertyManager->Tie("velocities/p-rad_sec", this, eP, (PMF)&FGPropagate::GetPQR);
687 : 1 : PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
688 : 1 : PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
689 : :
690 : 1 : PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
691 : 1 : PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
692 : 1 : PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
693 : :
694 : 1 : PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
695 : :
696 : 1 : PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
697 : 1 : PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGPropagate::GetPQRdot);
698 : 1 : PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGPropagate::GetPQRdot);
699 : :
700 : 1 : PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGPropagate::GetUVWdot);
701 : 1 : PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
702 : 1 : PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
703 : :
704 : 1 : PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
705 : 1 : PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
706 : 1 : PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
707 : 1 : PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
708 : 1 : PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
709 : 1 : PropertyManager->Tie("position/long-gc-deg", this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
710 : 1 : PropertyManager->Tie("position/lat-geod-rad", this, &FGPropagate::GetGeodLatitudeRad);
711 : 1 : PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
712 : 1 : PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
713 : 1 : PropertyManager->Tie("position/h-agl-ft", this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
714 : 1 : PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
715 : : PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
716 : : &FGPropagate::GetTerrainElevation,
717 : 1 : &FGPropagate::SetTerrainElevation, false);
718 : :
719 : 1 : PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
720 : :
721 : 1 : PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
722 : 1 : PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
723 : 1 : PropertyManager->Tie("attitude/psi-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
724 : :
725 : 1 : PropertyManager->Tie("attitude/roll-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
726 : 1 : PropertyManager->Tie("attitude/pitch-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);
727 : 1 : PropertyManager->Tie("attitude/heading-true-rad", this, (int)ePsi, (PMF)&FGPropagate::GetEuler);
728 : :
729 : 1 : PropertyManager->Tie("simulation/integrator/rate/rotational", (int*)&integrator_rotational_rate);
730 : 1 : PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate);
731 : 1 : PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position);
732 : 1 : PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position);
733 : 1 : PropertyManager->Tie("simulation/gravity-model", &gravType);
734 : 1 : }
735 : :
736 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
737 : : // The bitmasked value choices are as follows:
738 : : // unset: In this case (the default) JSBSim would only print
739 : : // out the normally expected messages, essentially echoing
740 : : // the config files as they are read. If the environment
741 : : // variable is not set, debug_lvl is set to 1 internally
742 : : // 0: This requests JSBSim not to output any messages
743 : : // whatsoever.
744 : : // 1: This value explicity requests the normal JSBSim
745 : : // startup messages
746 : : // 2: This value asks for a message to be printed out when
747 : : // a class is instantiated
748 : : // 4: When this value is set, a message is displayed when a
749 : : // FGModel object executes its Run() method
750 : : // 8: When this value is set, various runtime state variables
751 : : // are printed out periodically
752 : : // 16: When set various parameters are sanity checked and
753 : : // a message is printed out when they go out of bounds
754 : :
755 : 54008 : void FGPropagate::Debug(int from)
756 : : {
757 [ + - ]: 54008 : if (debug_lvl <= 0) return;
758 : :
759 : 54008 : if (debug_lvl & 1) { // Standard console startup message output
760 : : if (from == 0) { // Constructor
761 : :
762 : : }
763 : : }
764 [ - + ]: 54008 : if (debug_lvl & 2 ) { // Instantiation/Destruction notification
765 [ # # ]: 0 : if (from == 0) cout << "Instantiated: FGPropagate" << endl;
766 [ # # ]: 0 : if (from == 1) cout << "Destroyed: FGPropagate" << endl;
767 : : }
768 : 54008 : if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
769 : : }
770 [ - + ][ # # ]: 54008 : if (debug_lvl & 8 && from == 2) { // Runtime state variables
771 : : cout << endl << fgblue << highint << left
772 : : << " Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->GetSimTime() << " seconds"
773 : 0 : << reset << endl;
774 : : cout << endl;
775 : : cout << highint << " Earth Position Angle (deg): " << setw(8) << setprecision(3) << reset
776 : 0 : << Inertial->GetEarthPositionAngleDeg() << endl;
777 : : cout << endl;
778 : 0 : cout << highint << " Body velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vUVW << endl;
779 : 0 : cout << highint << " Local velocity (ft/sec): " << setw(8) << setprecision(3) << reset << vVel << endl;
780 : 0 : cout << highint << " Inertial velocity (ft/sec): " << setw(8) << setprecision(3) << reset << VState.vInertialVelocity << endl;
781 : 0 : cout << highint << " Inertial Position (ft): " << setw(10) << setprecision(3) << reset << VState.vInertialPosition << endl;
782 : 0 : cout << highint << " Latitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLatitudeDeg() << endl;
783 : 0 : cout << highint << " Longitude (deg): " << setw(8) << setprecision(3) << reset << VState.vLocation.GetLongitudeDeg() << endl;
784 : 0 : cout << highint << " Altitude ASL (ft): " << setw(8) << setprecision(3) << reset << GetAltitudeASL() << endl;
785 : 0 : cout << highint << " Acceleration (NED, ft/sec^2): " << setw(8) << setprecision(3) << reset << Tb2l*GetUVWdot() << endl;
786 : : cout << endl;
787 : : cout << highint << " Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
788 : 0 : << reset << endl << Tec2b.Dump("\t", " ") << endl;
789 : : cout << highint << " Associated Euler angles (deg): " << setw(8)
790 : : << setprecision(3) << reset << (Tec2b.GetQuaternion().GetEuler()*radtodeg)
791 : 0 : << endl << endl;
792 : :
793 : : cout << highint << " Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
794 : 0 : << reset << endl << Tb2ec.Dump("\t", " ") << endl;
795 : : cout << highint << " Associated Euler angles (deg): " << setw(8)
796 : : << setprecision(3) << reset << (Tb2ec.GetQuaternion().GetEuler()*radtodeg)
797 : 0 : << endl << endl;
798 : :
799 : : cout << highint << " Matrix Local to Body (Orientation of Body with respect to Local):"
800 : 0 : << reset << endl << Tl2b.Dump("\t", " ") << endl;
801 : : cout << highint << " Associated Euler angles (deg): " << setw(8)
802 : : << setprecision(3) << reset << (Tl2b.GetQuaternion().GetEuler()*radtodeg)
803 : 0 : << endl << endl;
804 : :
805 : : cout << highint << " Matrix Body to Local (Orientation of Local with respect to Body):"
806 : 0 : << reset << endl << Tb2l.Dump("\t", " ") << endl;
807 : : cout << highint << " Associated Euler angles (deg): " << setw(8)
808 : : << setprecision(3) << reset << (Tb2l.GetQuaternion().GetEuler()*radtodeg)
809 : 0 : << endl << endl;
810 : :
811 : : cout << highint << " Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
812 : 0 : << reset << endl << Tl2ec.Dump("\t", " ") << endl;
813 : : cout << highint << " Associated Euler angles (deg): " << setw(8)
814 : : << setprecision(3) << reset << (Tl2ec.GetQuaternion().GetEuler()*radtodeg)
815 : 0 : << endl << endl;
816 : :
817 : : cout << highint << " Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
818 : 0 : << reset << endl << Tec2l.Dump("\t", " ") << endl;
819 : : cout << highint << " Associated Euler angles (deg): " << setw(8)
820 : : << setprecision(3) << reset << (Tec2l.GetQuaternion().GetEuler()*radtodeg)
821 : 0 : << endl << endl;
822 : :
823 : : cout << highint << " Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
824 : 0 : << reset << endl << Tec2i.Dump("\t", " ") << endl;
825 : : cout << highint << " Associated Euler angles (deg): " << setw(8)
826 : : << setprecision(3) << reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
827 : 0 : << endl << endl;
828 : :
829 : : cout << highint << " Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
830 : 0 : << reset << endl << Ti2ec.Dump("\t", " ") << endl;
831 : : cout << highint << " Associated Euler angles (deg): " << setw(8)
832 : : << setprecision(3) << reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
833 : 0 : << endl << endl;
834 : :
835 : : cout << highint << " Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
836 : 0 : << reset << endl << Ti2b.Dump("\t", " ") << endl;
837 : : cout << highint << " Associated Euler angles (deg): " << setw(8)
838 : : << setprecision(3) << reset << (Ti2b.GetQuaternion().GetEuler()*radtodeg)
839 : 0 : << endl << endl;
840 : :
841 : : cout << highint << " Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
842 : 0 : << reset << endl << Tb2i.Dump("\t", " ") << endl;
843 : : cout << highint << " Associated Euler angles (deg): " << setw(8)
844 : : << setprecision(3) << reset << (Tb2i.GetQuaternion().GetEuler()*radtodeg)
845 : 0 : << endl << endl;
846 : :
847 : : cout << highint << " Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
848 : 0 : << reset << endl << Ti2l.Dump("\t", " ") << endl;
849 : : cout << highint << " Associated Euler angles (deg): " << setw(8)
850 : : << setprecision(3) << reset << (Ti2l.GetQuaternion().GetEuler()*radtodeg)
851 : 0 : << endl << endl;
852 : :
853 : : cout << highint << " Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
854 : 0 : << reset << endl << Tl2i.Dump("\t", " ") << endl;
855 : : cout << highint << " Associated Euler angles (deg): " << setw(8)
856 : : << setprecision(3) << reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
857 : 0 : << endl << endl;
858 : :
859 : 0 : cout << setprecision(6); // reset the output stream
860 : : }
861 [ - + ]: 54008 : if (debug_lvl & 16) { // Sanity checking
862 [ # # ]: 0 : if (from == 2) { // State sanity checking
863 [ # # ]: 0 : if (fabs(VState.vPQR.Magnitude()) > 1000.0) {
864 : 0 : cerr << endl << "Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.vPQR.Magnitude() << endl;
865 : 0 : exit(-1);
866 : : }
867 [ # # ]: 0 : if (fabs(VState.vUVW.Magnitude()) > 1.0e10) {
868 : 0 : cerr << endl << "Vehicle velocity is excessive (>1e10 ft/sec): " << VState.vUVW.Magnitude() << endl;
869 : 0 : exit(-1);
870 : : }
871 [ # # ]: 0 : if (fabs(GetDistanceAGL()) > 1e10) {
872 : 0 : cerr << endl << "Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL() << endl;
873 : 0 : exit(-1);
874 : : }
875 : : }
876 : : }
877 [ - + ]: 54008 : if (debug_lvl & 64) {
878 [ # # ]: 0 : if (from == 0) { // Constructor
879 : 0 : cout << IdSrc << endl;
880 : 0 : cout << IdHdr << endl;
881 : : }
882 : : }
883 : : }
884 [ + + ][ + - ]: 12 : }
|
