1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
7 ------------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) -------------
9 This program is free software; you can redistribute it and/or modify it under
10 the terms of the GNU Lesser General Public License as published by the Free Software
11 Foundation; either version 2 of the License, or (at your option) any later
14 This program is distributed in the hope that it will be useful, but WITHOUT
15 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
16 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
19 You should have received a copy of the GNU Lesser General Public License along with
20 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21 Place - Suite 330, Boston, MA 02111-1307, USA.
23 Further information about the GNU Lesser General Public License can also be found on
24 the world wide web at http://www.gnu.org.
27 --------------------------------------------------------------------------------
30 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
37 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
41 #include "models/FGModel.h"
42 #include "math/FGColumnVector3.h"
43 #include "math/FGLocation.h"
44 #include "math/FGQuaternion.h"
45 #include "math/FGMatrix33.h"
48 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
50 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
52 #define ID_PROPAGATE "$Id: FGPropagate.h,v 1.51 2010/10/07 03:45:40 jberndt Exp $"
54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
56 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
61 class FGInitialCondition;
63 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
65 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
67 /** Models the EOM and integration/propagation of state.
68 The Equations of Motion (EOM) for JSBSim are integrated to propagate the
69 state of the vehicle given the forces and moments that act on it. The
70 integration accounts for a rotating Earth.
72 The general execution of this model follows this process:
74 -Calculate the angular accelerations
75 -Calculate the translational accelerations
76 -Calculate the angular rate
77 -Calculate the translational velocity
79 -Integrate accelerations and rates
81 Integration of rotational and translation position and rate can be
82 customized as needed or frozen by the selection of no integrator. The
83 selection of which integrator to use is done through the setting of
84 the associated property. There are four properties which can be set:
87 simulation/integrator/rate/rotational
88 simulation/integrator/rate/translational
89 simulation/integrator/position/rotational
90 simulation/integrator/position/translational
93 Each of the integrators listed above can be set to one of the following values:
96 0: No integrator (Freeze)
104 @author Jon S. Berndt, Mathias Froehlich
105 @version $Id: FGPropagate.h,v 1.51 2010/10/07 03:45:40 jberndt Exp $
108 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
110 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
112 class FGPropagate : public FGModel {
115 /** The current vehicle state vector structure contains the translational and
116 angular position, and the translational and angular velocity. */
117 struct VehicleState {
118 /** Represents the current location of the vehicle in Earth centered Earth
121 FGLocation vLocation;
123 /** The velocity vector of the vehicle with respect to the ECEF frame,
124 expressed in the body system.
126 FGColumnVector3 vUVW;
128 /** The angular velocity vector for the vehicle relative to the ECEF frame,
129 expressed in the body frame.
131 FGColumnVector3 vPQR;
133 /** The angular velocity vector for the vehicle body frame relative to the
134 ECI frame, expressed in the body frame.
136 FGColumnVector3 vPQRi;
138 /** The angular velocity vector for the vehicle body frame relative to the
139 ECI frame, expressed in the ECI frame.
141 FGColumnVector3 vPQRi_i;
143 /** The current orientation of the vehicle, that is, the orientation of the
144 body frame relative to the local, NED frame. */
145 FGQuaternion qAttitudeLocal;
147 /** The current orientation of the vehicle, that is, the orientation of the
148 body frame relative to the inertial (ECI) frame. */
149 FGQuaternion qAttitudeECI;
151 FGColumnVector3 vInertialVelocity;
153 FGColumnVector3 vInertialPosition;
155 deque <FGColumnVector3> dqPQRdot;
156 deque <FGColumnVector3> dqPQRidot;
157 deque <FGColumnVector3> dqUVWidot;
158 deque <FGColumnVector3> dqInertialVelocity;
159 deque <FGQuaternion> dqQtrndot;
163 The constructor initializes several variables, and sets the initial set
164 of integrators to use as follows:
165 - integrator, rotational rate = Adams Bashforth 2
166 - integrator, translational rate = Adams Bashforth 2
167 - integrator, rotational position = Trapezoidal
168 - integrator, translational position = Trapezoidal
169 @param Executive a pointer to the parent executive object */
170 FGPropagate(FGFDMExec* Executive);
175 /// These define the indices use to select the various integrators.
176 enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
178 /// These define the indices use to select the gravitation models.
179 enum eGravType {gtStandard, gtWGS84};
181 /** Initializes the FGPropagate class after instantiation and prior to first execution.
182 The base class FGModel::InitModel is called first, initializing pointers to the
183 other FGModel objects (and others). */
184 bool InitModel(void);
186 /** Runs the Propagate model; called by the Executive.
187 @return false if no error */
190 const FGQuaternion& GetQuaterniondot(void) const {return vQtrndot;}
192 /** Retrieves the velocity vector.
193 The vector returned is represented by an FGColumnVector reference. The vector
194 for the velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
195 is 1-based, so that the first element can be retrieved using the "()" operator.
196 In other words, vVel(1) is Vnorth. Various convenience enumerators are defined
197 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
198 eNorth=1, eEast=2, eDown=3.
200 @return The vehicle velocity vector with respect to the Earth centered frame,
201 expressed in Local horizontal frame.
203 const FGColumnVector3& GetVel(void) const { return vVel; }
205 /** Retrieves the body frame vehicle velocity vector.
206 The vector returned is represented by an FGColumnVector reference. The vector
207 for the velocity in Body frame is organized (Vx, Vy, Vz). The vector
208 is 1-based, so that the first element can be retrieved using the "()" operator.
209 In other words, vUVW(1) is Vx. Various convenience enumerators are defined
210 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
213 @return The body frame vehicle velocity vector in ft/sec.
215 const FGColumnVector3& GetUVW(void) const { return VState.vUVW; }
217 /** Retrieves the body axis acceleration.
218 Retrieves the computed body axis accelerations based on the
219 applied forces and accounting for a rotating body frame.
220 The vector returned is represented by an FGColumnVector reference. The vector
221 for the acceleration in Body frame is organized (Ax, Ay, Az). The vector
222 is 1-based, so that the first element can be retrieved using the "()" operator.
223 In other words, vUVWdot(1) is Ax. Various convenience enumerators are defined
224 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
227 @return Body axis translational acceleration in ft/sec^2.
229 const FGColumnVector3& GetUVWdot(void) const { return vUVWdot; }
231 /** Retrieves the body angular rates vector, relative to the ECEF frame.
232 Retrieves the body angular rates (p, q, r), which are calculated by integration
233 of the angular acceleration.
234 The vector returned is represented by an FGColumnVector reference. The vector
235 for the angular velocity in Body frame is organized (P, Q, R). The vector
236 is 1-based, so that the first element can be retrieved using the "()" operator.
237 In other words, vPQR(1) is P. Various convenience enumerators are defined
238 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
241 @return The body frame angular rates in rad/sec.
243 const FGColumnVector3& GetPQR(void) const {return VState.vPQR;}
245 /** Retrieves the body angular rates vector, relative to the ECI (inertial) frame.
246 Retrieves the body angular rates (p, q, r), which are calculated by integration
247 of the angular acceleration.
248 The vector returned is represented by an FGColumnVector reference. The vector
249 for the angular velocity in Body frame is organized (P, Q, R). The vector
250 is 1-based, so that the first element can be retrieved using the "()" operator.
251 In other words, vPQR(1) is P. Various convenience enumerators are defined
252 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
255 @return The body frame inertial angular rates in rad/sec.
257 const FGColumnVector3& GetPQRi(void) const {return VState.vPQRi;}
259 /** Retrieves the body axis angular acceleration vector.
260 Retrieves the body axis angular acceleration vector in rad/sec^2. The
261 angular acceleration vector is determined from the applied forces and
262 accounts for a rotating frame.
263 The vector returned is represented by an FGColumnVector reference. The vector
264 for the angular acceleration in Body frame is organized (Pdot, Qdot, Rdot). The vector
265 is 1-based, so that the first element can be retrieved using the "()" operator.
266 In other words, vPQRdot(1) is Pdot. Various convenience enumerators are defined
267 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
270 @return The angular acceleration vector.
272 const FGColumnVector3& GetPQRdot(void) const {return vPQRdot;}
274 /** Retrieves the Euler angles that define the vehicle orientation.
275 Extracts the Euler angles from the quaternion that stores the orientation
276 in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The
277 vector returned is represented by an FGColumnVector reference. The vector
278 for the Euler angles is organized (Phi, Theta, Psi). The vector
279 is 1-based, so that the first element can be retrieved using the "()" operator.
280 In other words, the returned vector item with subscript (1) is Phi.
281 Various convenience enumerators are defined in FGJSBBase. The relevant
282 enumerators for the vector returned by this call are, ePhi=1, eTht=2, ePsi=3.
284 @return The Euler angle vector, where the first item in the
285 vector is the angle about the X axis, the second is the
286 angle about the Y axis, and the third item is the angle
287 about the Z axis (Phi, Theta, Psi).
289 const FGColumnVector3& GetEuler(void) const { return VState.qAttitudeLocal.GetEuler(); }
291 /** Retrieves a body frame velocity component.
292 Retrieves a body frame velocity component. The velocity returned is
293 extracted from the vUVW vector (an FGColumnVector). The vector for the
294 velocity in Body frame is organized (Vx, Vy, Vz). The vector is 1-based.
295 In other words, GetUVW(1) returns Vx. Various convenience enumerators
296 are defined in FGJSBBase. The relevant enumerators for the velocity
297 returned by this call are, eX=1, eY=2, eZ=3.
299 @param idx the index of the velocity component desired (1-based).
300 @return The body frame velocity component.
302 double GetUVW (int idx) const { return VState.vUVW(idx); }
304 /** Retrieves a body frame acceleration component.
305 Retrieves a body frame acceleration component. The acceleration returned
306 is extracted from the vUVWdot vector (an FGColumnVector). The vector for
307 the acceleration in Body frame is organized (Ax, Ay, Az). The vector is
308 1-based. In other words, GetUVWdot(1) returns Ax. Various convenience
309 enumerators are defined in FGJSBBase. The relevant enumerators for the
310 acceleration returned by this call are, eX=1, eY=2, eZ=3.
312 @param idx the index of the acceleration component desired (1-based).
313 @return The body frame acceleration component.
315 double GetUVWdot(int idx) const { return vUVWdot(idx); }
317 /** Retrieves a Local frame velocity component.
318 Retrieves a Local frame velocity component. The velocity returned is
319 extracted from the vVel vector (an FGColumnVector). The vector for the
320 velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
321 is 1-based. In other words, GetVel(1) returns Vnorth. Various convenience
322 enumerators are defined in FGJSBBase. The relevant enumerators for the
323 velocity returned by this call are, eNorth=1, eEast=2, eDown=3.
325 @param idx the index of the velocity component desired (1-based).
326 @return The body frame velocity component.
328 double GetVel(int idx) const { return vVel(idx); }
330 /** Retrieves the total inertial velocity in ft/sec.
332 double GetInertialVelocityMagnitude(void) const { return VState.vInertialVelocity.Magnitude(); }
334 /** Retrieves the inertial velocity vector in ft/sec.
336 const FGColumnVector3& GetInertialVelocity(void) const { return VState.vInertialVelocity; }
338 /** Retrieves the inertial position vector.
340 const FGColumnVector3& GetInertialPosition(void) const { return VState.vInertialPosition; }
342 /** Returns the current altitude above sea level.
343 This function returns the altitude above sea level.
345 @return The current altitude above sea level in feet.
347 double GetAltitudeASL(void) const { return VState.vLocation.GetRadius() - SeaLevelRadius; }
349 /** Returns the current altitude above sea level.
350 This function returns the altitude above sea level.
352 @return The current altitude above sea level in meters.
354 double GetAltitudeASLmeters(void) const { return GetAltitudeASL()*fttom;}
356 /** Retrieves a body frame angular velocity component relative to the ECEF frame.
357 Retrieves a body frame angular velocity component. The angular velocity
358 returned is extracted from the vPQR vector (an FGColumnVector). The vector
359 for the angular velocity in Body frame is organized (P, Q, R). The vector
360 is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
361 convenience enumerators are defined in FGJSBBase. The relevant enumerators
362 for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
364 @param axis the index of the angular velocity component desired (1-based).
365 @return The body frame angular velocity component.
367 double GetPQR(int axis) const {return VState.vPQR(axis);}
369 /** Retrieves a body frame angular velocity component relative to the ECI (inertial) frame.
370 Retrieves a body frame angular velocity component. The angular velocity
371 returned is extracted from the vPQR vector (an FGColumnVector). The vector
372 for the angular velocity in Body frame is organized (P, Q, R). The vector
373 is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
374 convenience enumerators are defined in FGJSBBase. The relevant enumerators
375 for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
377 @param axis the index of the angular velocity component desired (1-based).
378 @return The body frame angular velocity component.
380 double GetPQRi(int axis) const {return VState.vPQRi(axis);}
382 /** Retrieves a body frame angular acceleration component.
383 Retrieves a body frame angular acceleration component. The angular
384 acceleration returned is extracted from the vPQRdot vector (an
385 FGColumnVector). The vector for the angular acceleration in Body frame
386 is organized (Pdot, Qdot, Rdot). The vector is 1-based. In other words,
387 GetPQRdot(1) returns Pdot (roll acceleration). Various convenience
388 enumerators are defined in FGJSBBase. The relevant enumerators for the
389 angular acceleration returned by this call are, eP=1, eQ=2, eR=3.
391 @param axis the index of the angular acceleration component desired (1-based).
392 @return The body frame angular acceleration component.
394 double GetPQRdot(int axis) const {return vPQRdot(axis);}
396 /** Retrieves a vehicle Euler angle component.
397 Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that
398 stores the vehicle orientation relative to the Local frame. The order of
399 rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is
400 Phi. Various convenience enumerators are defined in FGJSBBase. The
401 relevant enumerators for the Euler angle returned by this call are,
402 ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta).
404 @return An Euler angle.
406 double GetEuler(int axis) const { return VState.qAttitudeLocal.GetEuler(axis); }
408 /** Retrieves the cosine of a vehicle Euler angle component.
409 Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
410 quaternion that stores the vehicle orientation relative to the Local frame.
411 The order of rotations used is Yaw-Pitch-Roll. The Euler angle
412 with subscript (1) is Phi. Various convenience enumerators are defined in
413 FGJSBBase. The relevant enumerators for the Euler angle referred to in this
414 call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetCosEuler(eTht) returns cos(theta)).
416 @return The cosine of an Euler angle.
418 double GetCosEuler(int idx) const { return VState.qAttitudeLocal.GetCosEuler(idx); }
420 /** Retrieves the sine of a vehicle Euler angle component.
421 Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the
422 quaternion that stores the vehicle orientation relative to the Local frame.
423 The order of rotations used is Yaw-Pitch-Roll. The Euler angle
424 with subscript (1) is Phi. Various convenience enumerators are defined in
425 FGJSBBase. The relevant enumerators for the Euler angle referred to in this
426 call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetSinEuler(eTht) returns sin(theta)).
428 @return The sine of an Euler angle.
430 double GetSinEuler(int idx) const { return VState.qAttitudeLocal.GetSinEuler(idx); }
432 /** Returns the current altitude rate.
433 Returns the current altitude rate (rate of climb).
435 @return The current rate of change in altitude.
437 double Gethdot(void) const { return -vVel(eDown); }
439 /** Returns the "constant" LocalTerrainRadius.
440 The LocalTerrainRadius parameter is set by the calling application or set to
441 sea level + terrain elevation if JSBSim is running in standalone mode.
443 @return distance of the local terrain from the center of the earth.
445 double GetLocalTerrainRadius(void) const;
447 double GetSeaLevelRadius(void) const { return SeaLevelRadius; }
448 double GetTerrainElevation(void) const;
449 double GetDistanceAGL(void) const;
450 double GetRadius(void) const {
451 if (VState.vLocation.GetRadius() == 0) return 1.0;
452 else return VState.vLocation.GetRadius();
454 double GetLongitude(void) const { return VState.vLocation.GetLongitude(); }
455 double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }
457 double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); }
458 double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }
460 double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }
462 double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
463 double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
464 const FGLocation& GetLocation(void) const { return VState.vLocation; }
466 /** Retrieves the local-to-body transformation matrix.
467 The quaternion class, being the means by which the orientation of the
468 vehicle is stored, manages the local-to-body transformation matrix.
469 @return a reference to the local-to-body transformation matrix. */
470 const FGMatrix33& GetTl2b(void) const { return VState.qAttitudeLocal.GetT(); }
472 /** Retrieves the body-to-local transformation matrix.
473 The quaternion class, being the means by which the orientation of the
474 vehicle is stored, manages the body-to-local transformation matrix.
475 @return a reference to the body-to-local matrix. */
476 const FGMatrix33& GetTb2l(void) const { return VState.qAttitudeLocal.GetTInv(); }
478 /** Retrieves the ECEF-to-body transformation matrix.
479 @return a reference to the ECEF-to-body transformation matrix. */
480 const FGMatrix33& GetTec2b(void) const { return Tec2b; }
482 /** Retrieves the body-to-ECEF transformation matrix.
483 @return a reference to the body-to-ECEF matrix. */
484 const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }
486 /** Retrieves the ECI-to-body transformation matrix.
487 @return a reference to the ECI-to-body transformation matrix. */
488 const FGMatrix33& GetTi2b(void) const { return VState.qAttitudeECI.GetT(); }
490 /** Retrieves the body-to-ECI transformation matrix.
491 @return a reference to the body-to-ECI matrix. */
492 const FGMatrix33& GetTb2i(void) const { return VState.qAttitudeECI.GetTInv(); }
494 /** Retrieves the ECEF-to-ECI transformation matrix.
495 @return a reference to the ECEF-to-ECI transformation matrix. */
496 const FGMatrix33& GetTec2i(void);
498 /** Retrieves the ECI-to-ECEF transformation matrix.
499 @return a reference to the ECI-to-ECEF matrix. */
500 const FGMatrix33& GetTi2ec(void);
502 /** Retrieves the ECEF-to-local transformation matrix.
503 Retrieves the ECEF-to-local transformation matrix. Note that the so-called
504 local from is also know as the NED frame (for North, East, Down).
505 @return a reference to the ECEF-to-local matrix. */
506 const FGMatrix33& GetTec2l(void) const { return VState.vLocation.GetTec2l(); }
508 /** Retrieves the local-to-ECEF transformation matrix.
509 Retrieves the local-to-ECEF transformation matrix. Note that the so-called
510 local from is also know as the NED frame (for North, East, Down).
511 @return a reference to the local-to-ECEF matrix. */
512 const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); }
514 /** Retrieves the local-to-inertial transformation matrix.
515 @return a reference to the local-to-inertial transformation matrix. */
516 const FGMatrix33& GetTl2i(void) { return VState.vLocation.GetTl2i(); }
518 /** Retrieves the inertial-to-local transformation matrix.
519 @return a reference to the inertial-to-local matrix. */
520 const FGMatrix33& GetTi2l(void) { return VState.vLocation.GetTi2l(); }
522 VehicleState* GetVState(void) { return &VState; }
524 void SetVState(VehicleState* vstate) {
525 VState.vLocation = vstate->vLocation;
526 UpdateLocationMatrices();
527 SetInertialOrientation(vstate->qAttitudeECI);
528 VehicleRadius = GetRadius();
529 VState.vUVW = vstate->vUVW;
530 vVel = GetTb2l() * VState.vUVW;
531 VState.vPQR = vstate->vPQR;
532 VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
533 VState.vInertialPosition = vstate->vInertialPosition;
535 InitializeDerivatives();
538 void InitializeDerivatives(void);
540 void SetInertialOrientation(FGQuaternion Qi);
541 void SetInertialVelocity(FGColumnVector3 Vi);
542 void SetInertialRates(FGColumnVector3 vRates);
544 const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
546 void SetPQR(unsigned int i, double val) {
547 if ((i>=1) && (i<=3) )
548 VState.vPQR(i) = val;
551 void SetUVW(unsigned int i, double val) {
552 if ((i>=1) && (i<=3) )
553 VState.vUVW(i) = val;
558 void SetLongitude(double lon) {
559 VState.vLocation.SetLongitude(lon);
560 VState.vInertialPosition = GetTec2i() * VState.vLocation;
561 UpdateLocationMatrices();
563 void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); }
564 void SetLatitude(double lat) {
565 VState.vLocation.SetLatitude(lat);
566 VState.vInertialPosition = GetTec2i() * VState.vLocation;
567 UpdateLocationMatrices();
569 void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); }
570 void SetRadius(double r) {
571 VState.vLocation.SetRadius(r);
572 VState.vInertialPosition = GetTec2i() * VState.vLocation;
573 UpdateLocationMatrices();
575 void SetAltitudeASL(double altASL);
576 void SetAltitudeASLmeters(double altASL) {SetAltitudeASL(altASL/fttom);}
577 void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
578 void SetTerrainElevation(double tt);
579 void SetDistanceAGL(double tt);
580 void SetInitialState(const FGInitialCondition *);
581 void SetPosition(const double Lon, const double Lat, const double Radius) {
582 VState.vLocation.SetPosition(Lon, Lat, Radius);
583 VState.vInertialPosition = GetTec2i() * VState.vLocation;
584 VehicleRadius = GetRadius();
585 UpdateLocationMatrices();
587 void SetLocation(const FGLocation& l) {
588 VState.vLocation = l;
589 VState.vInertialPosition = GetTec2i() * VState.vLocation;
590 UpdateLocationMatrices();
592 void SetLocation(const FGColumnVector3& l) {
593 VState.vLocation = l;
594 VState.vInertialPosition = GetTec2i() * VState.vLocation;
595 UpdateLocationMatrices();
598 void RecomputeLocalTerrainRadius(void);
600 void NudgeBodyLocation(FGColumnVector3 deltaLoc) {
601 vDeltaXYZEC = GetTb2ec()*deltaLoc;
602 VState.vLocation -= vDeltaXYZEC;
605 struct LagrangeMultiplier {
606 FGColumnVector3 ForceJacobian;
607 FGColumnVector3 MomentJacobian;
613 void DumpState(void);
619 struct VehicleState VState;
621 FGColumnVector3 vVel;
622 FGColumnVector3 vPQRdot;
623 FGColumnVector3 vPQRidot;
624 FGColumnVector3 vUVWdot, vUVWidot;
625 FGColumnVector3 vInertialVelocity;
626 FGColumnVector3 vLocation;
627 FGColumnVector3 vDeltaXYZEC;
628 FGColumnVector3 vGravAccel;
629 FGColumnVector3 vOmegaEarth; // The Earth angular velocity vector
630 FGQuaternion vQtrndot;
633 FGMatrix33 Tl2b; // local to body frame matrix copy for immediate local use
634 FGMatrix33 Tb2l; // body to local frame matrix copy for immediate local use
635 FGMatrix33 Tl2ec; // local to ECEF matrix copy for immediate local use
636 FGMatrix33 Tec2l; // ECEF to local frame matrix copy for immediate local use
637 FGMatrix33 Tec2i; // ECEF to ECI frame matrix copy for immediate local use
638 FGMatrix33 Ti2ec; // ECI to ECEF frame matrix copy for immediate local use
639 FGMatrix33 Ti2b; // ECI to body frame rotation matrix
640 FGMatrix33 Tb2i; // body to ECI frame rotation matrix
644 double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
645 FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity;
646 eIntegrateType integrator_rotational_rate;
647 eIntegrateType integrator_translational_rate;
648 eIntegrateType integrator_rotational_position;
649 eIntegrateType integrator_translational_position;
652 void CalculatePQRdot(void);
653 void CalculateQuatdot(void);
654 void CalculateInertialVelocity(void);
655 void CalculateUVW(void);
656 void CalculateUVWdot(void);
658 void Integrate( FGColumnVector3& Integrand,
659 FGColumnVector3& Val,
660 deque <FGColumnVector3>& ValDot,
662 eIntegrateType integration_type);
664 void Integrate( FGQuaternion& Integrand,
666 deque <FGQuaternion>& ValDot,
668 eIntegrateType integration_type);
670 void ResolveFrictionForces(double dt);
672 void UpdateLocationMatrices(void);
673 void UpdateBodyMatrices(void);
676 void Debug(int from);
679 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
681 #include "initialization/FGInitialCondition.h"