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.48 2010/09/18 22:48:12 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.48 2010/09/18 22:48:12 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 current orientation of the vehicle, that is, the orientation of the
139 body frame relative to the local, NED frame. */
140 FGQuaternion qAttitudeLocal;
142 /** The current orientation of the vehicle, that is, the orientation of the
143 body frame relative to the inertial (ECI) frame. */
144 FGQuaternion qAttitudeECI;
146 FGColumnVector3 vInertialVelocity;
148 FGColumnVector3 vInertialPosition;
150 deque <FGColumnVector3> dqPQRdot;
151 deque <FGColumnVector3> dqUVWidot;
152 deque <FGColumnVector3> dqInertialVelocity;
153 deque <FGQuaternion> dqQtrndot;
157 The constructor initializes several variables, and sets the initial set
158 of integrators to use as follows:
159 - integrator, rotational rate = Adams Bashforth 2
160 - integrator, translational rate = Adams Bashforth 2
161 - integrator, rotational position = Trapezoidal
162 - integrator, translational position = Trapezoidal
163 @param Executive a pointer to the parent executive object */
164 FGPropagate(FGFDMExec* Executive);
169 /// These define the indices use to select the various integrators.
170 enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4};
172 /// These define the indices use to select the gravitation models.
173 enum eGravType {gtStandard, gtWGS84};
175 /** Initializes the FGPropagate class after instantiation and prior to first execution.
176 The base class FGModel::InitModel is called first, initializing pointers to the
177 other FGModel objects (and others). */
178 bool InitModel(void);
180 /** Runs the Propagate model; called by the Executive.
181 @return false if no error */
184 const FGQuaternion& GetQuaterniondot(void) const {return vQtrndot;}
186 /** Retrieves the velocity vector.
187 The vector returned is represented by an FGColumnVector reference. The vector
188 for the velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
189 is 1-based, so that the first element can be retrieved using the "()" operator.
190 In other words, vVel(1) is Vnorth. Various convenience enumerators are defined
191 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
192 eNorth=1, eEast=2, eDown=3.
194 @return The vehicle velocity vector with respect to the Earth centered frame,
195 expressed in Local horizontal frame.
197 const FGColumnVector3& GetVel(void) const { return vVel; }
199 /** Retrieves the body frame vehicle velocity vector.
200 The vector returned is represented by an FGColumnVector reference. The vector
201 for the velocity in Body frame is organized (Vx, Vy, Vz). The vector
202 is 1-based, so that the first element can be retrieved using the "()" operator.
203 In other words, vUVW(1) is Vx. Various convenience enumerators are defined
204 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
207 @return The body frame vehicle velocity vector in ft/sec.
209 const FGColumnVector3& GetUVW(void) const { return VState.vUVW; }
211 /** Retrieves the body axis acceleration.
212 Retrieves the computed body axis accelerations based on the
213 applied forces and accounting for a rotating body frame.
214 The vector returned is represented by an FGColumnVector reference. The vector
215 for the acceleration in Body frame is organized (Ax, Ay, Az). The vector
216 is 1-based, so that the first element can be retrieved using the "()" operator.
217 In other words, vUVWdot(1) is Ax. Various convenience enumerators are defined
218 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
221 @return Body axis translational acceleration in ft/sec^2.
223 const FGColumnVector3& GetUVWdot(void) const { return vUVWdot; }
225 /** Retrieves the body angular rates vector, relative to the ECEF frame.
226 Retrieves the body angular rates (p, q, r), which are calculated by integration
227 of the angular acceleration.
228 The vector returned is represented by an FGColumnVector reference. The vector
229 for the angular velocity in Body frame is organized (P, Q, R). The vector
230 is 1-based, so that the first element can be retrieved using the "()" operator.
231 In other words, vPQR(1) is P. Various convenience enumerators are defined
232 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
235 @return The body frame angular rates in rad/sec.
237 const FGColumnVector3& GetPQR(void) const {return VState.vPQR;}
239 /** Retrieves the body angular rates vector, relative to the ECI (inertial) frame.
240 Retrieves the body angular rates (p, q, r), which are calculated by integration
241 of the angular acceleration.
242 The vector returned is represented by an FGColumnVector reference. The vector
243 for the angular velocity in Body frame is organized (P, Q, R). The vector
244 is 1-based, so that the first element can be retrieved using the "()" operator.
245 In other words, vPQR(1) is P. Various convenience enumerators are defined
246 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
249 @return The body frame inertial angular rates in rad/sec.
251 const FGColumnVector3& GetPQRi(void) const {return VState.vPQRi;}
253 /** Retrieves the body axis angular acceleration vector.
254 Retrieves the body axis angular acceleration vector in rad/sec^2. The
255 angular acceleration vector is determined from the applied forces and
256 accounts for a rotating frame.
257 The vector returned is represented by an FGColumnVector reference. The vector
258 for the angular acceleration in Body frame is organized (Pdot, Qdot, Rdot). The vector
259 is 1-based, so that the first element can be retrieved using the "()" operator.
260 In other words, vPQRdot(1) is Pdot. Various convenience enumerators are defined
261 in FGJSBBase. The relevant enumerators for the vector returned by this call are,
264 @return The angular acceleration vector.
266 const FGColumnVector3& GetPQRdot(void) const {return vPQRdot;}
268 /** Retrieves the Euler angles that define the vehicle orientation.
269 Extracts the Euler angles from the quaternion that stores the orientation
270 in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The
271 vector returned is represented by an FGColumnVector reference. The vector
272 for the Euler angles is organized (Phi, Theta, Psi). The vector
273 is 1-based, so that the first element can be retrieved using the "()" operator.
274 In other words, the returned vector item with subscript (1) is Phi.
275 Various convenience enumerators are defined in FGJSBBase. The relevant
276 enumerators for the vector returned by this call are, ePhi=1, eTht=2, ePsi=3.
278 @return The Euler angle vector, where the first item in the
279 vector is the angle about the X axis, the second is the
280 angle about the Y axis, and the third item is the angle
281 about the Z axis (Phi, Theta, Psi).
283 const FGColumnVector3& GetEuler(void) const { return VState.qAttitudeLocal.GetEuler(); }
285 /** Retrieves a body frame velocity component.
286 Retrieves a body frame velocity component. The velocity returned is
287 extracted from the vUVW vector (an FGColumnVector). The vector for the
288 velocity in Body frame is organized (Vx, Vy, Vz). The vector is 1-based.
289 In other words, GetUVW(1) returns Vx. Various convenience enumerators
290 are defined in FGJSBBase. The relevant enumerators for the velocity
291 returned by this call are, eX=1, eY=2, eZ=3.
293 @param idx the index of the velocity component desired (1-based).
294 @return The body frame velocity component.
296 double GetUVW (int idx) const { return VState.vUVW(idx); }
298 /** Retrieves a body frame acceleration component.
299 Retrieves a body frame acceleration component. The acceleration returned
300 is extracted from the vUVWdot vector (an FGColumnVector). The vector for
301 the acceleration in Body frame is organized (Ax, Ay, Az). The vector is
302 1-based. In other words, GetUVWdot(1) returns Ax. Various convenience
303 enumerators are defined in FGJSBBase. The relevant enumerators for the
304 acceleration returned by this call are, eX=1, eY=2, eZ=3.
306 @param idx the index of the acceleration component desired (1-based).
307 @return The body frame acceleration component.
309 double GetUVWdot(int idx) const { return vUVWdot(idx); }
311 /** Retrieves a Local frame velocity component.
312 Retrieves a Local frame velocity component. The velocity returned is
313 extracted from the vVel vector (an FGColumnVector). The vector for the
314 velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector
315 is 1-based. In other words, GetVel(1) returns Vnorth. Various convenience
316 enumerators are defined in FGJSBBase. The relevant enumerators for the
317 velocity returned by this call are, eNorth=1, eEast=2, eDown=3.
319 @param idx the index of the velocity component desired (1-based).
320 @return The body frame velocity component.
322 double GetVel(int idx) const { return vVel(idx); }
324 /** Retrieves the total inertial velocity in ft/sec.
326 double GetInertialVelocityMagnitude(void) const { return VState.vInertialVelocity.Magnitude(); }
328 /** Retrieves the inertial velocity vector in ft/sec.
330 const FGColumnVector3& GetInertialVelocity(void) const { return VState.vInertialVelocity; }
332 /** Retrieves the inertial position vector.
334 const FGColumnVector3& GetInertialPosition(void) const { return VState.vInertialPosition; }
336 /** Returns the current altitude above sea level.
337 This function returns the altitude above sea level.
339 @return The current altitude above sea level in feet.
341 double GetAltitudeASL(void) const { return VState.vLocation.GetRadius() - SeaLevelRadius; }
343 /** Returns the current altitude above sea level.
344 This function returns the altitude above sea level.
346 @return The current altitude above sea level in meters.
348 double GetAltitudeASLmeters(void) const { return GetAltitudeASL()*fttom;}
350 /** Retrieves a body frame angular velocity component relative to the ECEF frame.
351 Retrieves a body frame angular velocity component. The angular velocity
352 returned is extracted from the vPQR vector (an FGColumnVector). The vector
353 for the angular velocity in Body frame is organized (P, Q, R). The vector
354 is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
355 convenience enumerators are defined in FGJSBBase. The relevant enumerators
356 for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
358 @param axis the index of the angular velocity component desired (1-based).
359 @return The body frame angular velocity component.
361 double GetPQR(int axis) const {return VState.vPQR(axis);}
363 /** Retrieves a body frame angular velocity component relative to the ECI (inertial) frame.
364 Retrieves a body frame angular velocity component. The angular velocity
365 returned is extracted from the vPQR vector (an FGColumnVector). The vector
366 for the angular velocity in Body frame is organized (P, Q, R). The vector
367 is 1-based. In other words, GetPQR(1) returns P (roll rate). Various
368 convenience enumerators are defined in FGJSBBase. The relevant enumerators
369 for the angular velocity returned by this call are, eP=1, eQ=2, eR=3.
371 @param axis the index of the angular velocity component desired (1-based).
372 @return The body frame angular velocity component.
374 double GetPQRi(int axis) const {return VState.vPQRi(axis);}
376 /** Retrieves a body frame angular acceleration component.
377 Retrieves a body frame angular acceleration component. The angular
378 acceleration returned is extracted from the vPQRdot vector (an
379 FGColumnVector). The vector for the angular acceleration in Body frame
380 is organized (Pdot, Qdot, Rdot). The vector is 1-based. In other words,
381 GetPQRdot(1) returns Pdot (roll acceleration). Various convenience
382 enumerators are defined in FGJSBBase. The relevant enumerators for the
383 angular acceleration returned by this call are, eP=1, eQ=2, eR=3.
385 @param axis the index of the angular acceleration component desired (1-based).
386 @return The body frame angular acceleration component.
388 double GetPQRdot(int axis) const {return vPQRdot(axis);}
390 /** Retrieves a vehicle Euler angle component.
391 Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that
392 stores the vehicle orientation relative to the Local frame. The order of
393 rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is
394 Phi. Various convenience enumerators are defined in FGJSBBase. The
395 relevant enumerators for the Euler angle returned by this call are,
396 ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta).
398 @return An Euler angle.
400 double GetEuler(int axis) const { return VState.qAttitudeLocal.GetEuler(axis); }
402 /** Retrieves the cosine of a vehicle Euler angle component.
403 Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the
404 quaternion that stores the vehicle orientation relative to the Local frame.
405 The order of rotations used is Yaw-Pitch-Roll. The Euler angle
406 with subscript (1) is Phi. Various convenience enumerators are defined in
407 FGJSBBase. The relevant enumerators for the Euler angle referred to in this
408 call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetCosEuler(eTht) returns cos(theta)).
410 @return The cosine of an Euler angle.
412 double GetCosEuler(int idx) const { return VState.qAttitudeLocal.GetCosEuler(idx); }
414 /** Retrieves the sine of a vehicle Euler angle component.
415 Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the
416 quaternion that stores the vehicle orientation relative to the Local frame.
417 The order of rotations used is Yaw-Pitch-Roll. The Euler angle
418 with subscript (1) is Phi. Various convenience enumerators are defined in
419 FGJSBBase. The relevant enumerators for the Euler angle referred to in this
420 call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetSinEuler(eTht) returns sin(theta)).
422 @return The sine of an Euler angle.
424 double GetSinEuler(int idx) const { return VState.qAttitudeLocal.GetSinEuler(idx); }
426 /** Returns the current altitude rate.
427 Returns the current altitude rate (rate of climb).
429 @return The current rate of change in altitude.
431 double Gethdot(void) const { return -vVel(eDown); }
433 /** Returns the "constant" LocalTerrainRadius.
434 The LocalTerrainRadius parameter is set by the calling application or set to
435 sea level + terrain elevation if JSBSim is running in standalone mode.
437 @return distance of the local terrain from the center of the earth.
439 double GetLocalTerrainRadius(void) const;
441 double GetSeaLevelRadius(void) const { return SeaLevelRadius; }
442 double GetTerrainElevation(void) const;
443 double GetDistanceAGL(void) const;
444 double GetRadius(void) const {
445 if (VState.vLocation.GetRadius() == 0) return 1.0;
446 else return VState.vLocation.GetRadius();
448 double GetLongitude(void) const { return VState.vLocation.GetLongitude(); }
449 double GetLatitude(void) const { return VState.vLocation.GetLatitude(); }
451 double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); }
452 double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }
454 double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }
456 double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
457 double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
458 const FGLocation& GetLocation(void) const { return VState.vLocation; }
460 /** Retrieves the local-to-body transformation matrix.
461 The quaternion class, being the means by which the orientation of the
462 vehicle is stored, manages the local-to-body transformation matrix.
463 @return a reference to the local-to-body transformation matrix. */
464 const FGMatrix33& GetTl2b(void) const { return VState.qAttitudeLocal.GetT(); }
466 /** Retrieves the body-to-local transformation matrix.
467 The quaternion class, being the means by which the orientation of the
468 vehicle is stored, manages the body-to-local transformation matrix.
469 @return a reference to the body-to-local matrix. */
470 const FGMatrix33& GetTb2l(void) const { return VState.qAttitudeLocal.GetTInv(); }
472 /** Retrieves the ECEF-to-body transformation matrix.
473 @return a reference to the ECEF-to-body transformation matrix. */
474 const FGMatrix33& GetTec2b(void) const { return Tec2b; }
476 /** Retrieves the body-to-ECEF transformation matrix.
477 @return a reference to the body-to-ECEF matrix. */
478 const FGMatrix33& GetTb2ec(void) const { return Tb2ec; }
480 /** Retrieves the ECI-to-body transformation matrix.
481 @return a reference to the ECI-to-body transformation matrix. */
482 const FGMatrix33& GetTi2b(void) const { return VState.qAttitudeECI.GetT(); }
484 /** Retrieves the body-to-ECI transformation matrix.
485 @return a reference to the body-to-ECI matrix. */
486 const FGMatrix33& GetTb2i(void) const { return VState.qAttitudeECI.GetTInv(); }
488 /** Retrieves the ECEF-to-ECI transformation matrix.
489 @return a reference to the ECEF-to-ECI transformation matrix. */
490 const FGMatrix33& GetTec2i(void);
492 /** Retrieves the ECI-to-ECEF transformation matrix.
493 @return a reference to the ECI-to-ECEF matrix. */
494 const FGMatrix33& GetTi2ec(void);
496 /** Retrieves the ECEF-to-local transformation matrix.
497 Retrieves the ECEF-to-local transformation matrix. Note that the so-called
498 local from is also know as the NED frame (for North, East, Down).
499 @return a reference to the ECEF-to-local matrix. */
500 const FGMatrix33& GetTec2l(void) const { return VState.vLocation.GetTec2l(); }
502 /** Retrieves the local-to-ECEF transformation matrix.
503 Retrieves the local-to-ECEF 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 local-to-ECEF matrix. */
506 const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); }
508 /** Retrieves the local-to-inertial transformation matrix.
509 @return a reference to the local-to-inertial transformation matrix. */
510 const FGMatrix33& GetTl2i(void) { return VState.vLocation.GetTl2i(); }
512 /** Retrieves the inertial-to-local transformation matrix.
513 @return a reference to the inertial-to-local matrix. */
514 const FGMatrix33& GetTi2l(void) { return VState.vLocation.GetTi2l(); }
516 VehicleState* GetVState(void) { return &VState; }
518 void SetVState(VehicleState* vstate) {
519 VState.vLocation = vstate->vLocation;
520 UpdateLocationMatrices();
521 SetInertialOrientation(vstate->qAttitudeECI);
522 VehicleRadius = GetRadius();
523 VState.vUVW = vstate->vUVW;
524 vVel = GetTb2l() * VState.vUVW;
525 VState.vPQR = vstate->vPQR;
526 VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
527 VState.vInertialPosition = vstate->vInertialPosition;
529 InitializeDerivatives();
532 void InitializeDerivatives(void);
534 void SetInertialOrientation(FGQuaternion Qi);
535 void SetInertialVelocity(FGColumnVector3 Vi);
536 void SetInertialRates(FGColumnVector3 vRates);
538 const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; }
540 void SetPQR(unsigned int i, double val) {
541 if ((i>=1) && (i<=3) )
542 VState.vPQR(i) = val;
545 void SetUVW(unsigned int i, double val) {
546 if ((i>=1) && (i<=3) )
547 VState.vUVW(i) = val;
552 void SetLongitude(double lon) {
553 VState.vLocation.SetLongitude(lon);
554 VState.vInertialPosition = GetTec2i() * VState.vLocation;
555 UpdateLocationMatrices();
557 void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); }
558 void SetLatitude(double lat) {
559 VState.vLocation.SetLatitude(lat);
560 VState.vInertialPosition = GetTec2i() * VState.vLocation;
561 UpdateLocationMatrices();
563 void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); }
564 void SetRadius(double r) {
565 VState.vLocation.SetRadius(r);
566 VState.vInertialPosition = GetTec2i() * VState.vLocation;
567 UpdateLocationMatrices();
569 void SetLocation(const FGLocation& l) {
570 VState.vLocation = l;
571 VState.vInertialPosition = GetTec2i() * VState.vLocation;
572 UpdateLocationMatrices();
574 void SetLocation(const FGColumnVector3& l) {
575 VState.vLocation = l;
576 VState.vInertialPosition = GetTec2i() * VState.vLocation;
577 UpdateLocationMatrices();
579 void SetAltitudeASL(double altASL);
580 void SetAltitudeASLmeters(double altASL) {SetAltitudeASL(altASL/fttom);}
581 void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; }
582 void SetTerrainElevation(double tt);
583 void SetDistanceAGL(double tt);
584 void SetInitialState(const FGInitialCondition *);
585 void SetPosition(const double Lon, const double Lat, const double Radius) {
586 VState.vLocation.SetPosition(Lon, Lat, Radius);
587 VState.vInertialPosition = GetTec2i() * VState.vLocation;
588 VehicleRadius = GetRadius();
589 UpdateLocationMatrices();
592 void RecomputeLocalTerrainRadius(void);
594 void NudgeBodyLocation(FGColumnVector3 deltaLoc) {
595 vDeltaXYZEC = GetTb2ec()*deltaLoc;
596 VState.vLocation -= vDeltaXYZEC;
599 struct LagrangeMultiplier {
600 FGColumnVector3 ForceJacobian;
601 FGColumnVector3 MomentJacobian;
611 struct VehicleState VState;
613 FGColumnVector3 vVel;
614 FGColumnVector3 vPQRdot;
615 FGColumnVector3 vUVWdot, vUVWidot;
616 FGColumnVector3 vInertialVelocity;
617 FGColumnVector3 vLocation;
618 FGColumnVector3 vDeltaXYZEC;
619 FGColumnVector3 vGravAccel;
620 FGColumnVector3 vOmegaEarth; // The Earth angular velocity vector
621 FGQuaternion vQtrndot;
624 FGMatrix33 Tl2b; // local to body frame matrix copy for immediate local use
625 FGMatrix33 Tb2l; // body to local frame matrix copy for immediate local use
626 FGMatrix33 Tl2ec; // local to ECEF matrix copy for immediate local use
627 FGMatrix33 Tec2l; // ECEF to local frame matrix copy for immediate local use
628 FGMatrix33 Tec2i; // ECEF to ECI frame matrix copy for immediate local use
629 FGMatrix33 Ti2ec; // ECI to ECEF frame matrix copy for immediate local use
630 FGMatrix33 Ti2b; // ECI to body frame rotation matrix
631 FGMatrix33 Tb2i; // body to ECI frame rotation matrix
635 double LocalTerrainRadius, SeaLevelRadius, VehicleRadius;
636 FGColumnVector3 LocalTerrainVelocity;
637 eIntegrateType integrator_rotational_rate;
638 eIntegrateType integrator_translational_rate;
639 eIntegrateType integrator_rotational_position;
640 eIntegrateType integrator_translational_position;
643 void CalculatePQRdot(void);
644 void CalculateQuatdot(void);
645 void CalculateInertialVelocity(void);
646 void CalculateUVW(void);
647 void CalculateUVWdot(void);
649 void Integrate( FGColumnVector3& Integrand,
650 FGColumnVector3& Val,
651 deque <FGColumnVector3>& ValDot,
653 eIntegrateType integration_type);
655 void Integrate( FGQuaternion& Integrand,
657 deque <FGQuaternion>& ValDot,
659 eIntegrateType integration_type);
661 void ResolveFrictionForces(double dt);
663 void UpdateLocationMatrices(void);
664 void UpdateBodyMatrices(void);
667 void Debug(int from);
670 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
672 #include "initialization/FGInitialCondition.h"